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INTRODUCTION 

With  the  advancements  in  vision  systems  and  terrain 
preview  algorithms,  vehicles  with  on-board  automatic 
guidance  systems  are  becoming  of  greater  interest.   Uses  for 
these  types  of  vehicles  range  from  monitoring  hazardous 
locations  in  plants,  to  military  surveillance,  to  mobile 
instrument  platforms  on  distant  planets.   Walking  robots  are 
of  particular  interest  for  these  types  of  uses  because  of 
their  great  mobility.   The  ability  to  climb  over  obstacles, 
cross  ditches,  and  manipulate  objects  with  their  legs  are 
arguments  for  the  use  of  walking  robots  instead  of  wheeled 
vehicles.   It  is  interesting  to  note  that  there  are  many 
forms  of  naturally  occurring  legged  creatures,  but  no 
naturally  occurring  wheeled  creatures. 

The  purpose  of  this  work  is  to  develop  a  tool 
(simulation  model)  for  the  study  of  the  application  of 
advanced  control  concepts  to  the  guidance  and  control  of 
articulated  devices,  to  develop  and  experiment  with  a 
recursive  spline  path  definition  concept,  and  to  explore  a 
novel  feedforward  control  concept.   The  tool  which  is 
developed  is  a  simulation  of  a  walking  robot  which  has  a 
short  cylindrical  body  with  six,  three  jointed  legs 
positioned  symmetrically  about  the  body. 

The  kinematic  and  inverse  kinematic  equations  for  the 
legs  are  developed  in  Chapter  1.   These  equations  are  used 

1 


in  Chapter  2  in  developing  "the  dynamic  model,  in  the 
simulation  program  to  monitor  the  end  positions  of  the 
non-supporting  legs,  and  in  developing  the  joint  angle 
spline  curves. 

The  dynamic  model  is  developed  in  Chapter  2.   In  order 
to  reduce  the  complexity  of  the  mathematical  model,  the 
supporting  legs  are  treated  as  kinematic  linkages  and  the 
non-supporting  legs  are  treated  as  if  they  were  on  an 
inertial  reference  frame.   Lagrange's  equations  are  used  in 
writing  the  dynamics  for  the  body  and  for  the  non-supporting 
legs.   The  nine  constraint  equations  required  for  the  body 
dynamics  are  obtained  by  requiring  that  the  endpoints  of  the 
supporting  legs  remain  at  fixed  locations  in  the  world 
coordinate  system. 

The  recursive  polynomial  splines  used  to  define  the 
motion  of  the  body  and  of  the  joints  of  the  non-supporting 
legs  are  developed  in  Chapter  3.   A  recursive  formula  is 
devised  for  computing  the  coefficients  of  the  spline 
functions.   This  recursive  formula  eliminates  the  need  to 
perform  a  matrix  inversion  when  solving  for  the  spline 
coefficients.   The  recursive  nature  of  the  algorithm 
provides  a  spline  curve  which  passes  through  a  large  number 
of  data  points  but  uses  only  a  few  data  points  at  a  time. 
This  concept  is  important  for  real  time  control  applications 
where  the  desired  path  is  generated  in  real  time  from 
sensory  or  operator  inputs. 
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The  control  system  used  in  the  simulation  is  developed 
in  Chapter  4.   The  use  of  polynomial  spline  functions  to 
define  the  motion  of  the  body  and  legs  provides  a  command 
input  signal  which  allows  the  use  of  a  novel  feedforward 
control  concept.   A  study  of  the  performance  of  this  control 
concept  is  one  of  the  objectives  of  this  work.   Also,  a 
method  is  developed  which  allows  the  use  of  the  body 
position  and  orientation  in  computing  the  joint  torques  of 
the  supporting  legs.   The  corrective  body  forces  are 
computed  using  a  control  system  with  a  novel  feedforward 
feature.   The  supporting  leg  joint  torques  are  calculated 
using  the  Lagrangian  multipliers  and  constraint  equations 
developed  in  Chapter  2.   Calculating  the  supporting  leg 
joint  torques  in  this  manner  causes  all  of  the  supporting 
legs  to  act  in  unison  in  driving  the  body  to  its  correct 
position. 

The  performance  of  the  simulation  is  presented  in 
Chapter  5.   The  body  follows  the  position  and  orientation 
inputs  with  very  little  error.   The  joints  of  the 
non-supporting  legs  follow  their  inputs  closely  but  not  with 
the  accuracy  of  the  body,  indicating  the  need  for  more 
development  of  the  control  system  for  the  non-supporting 
legs. 


I.  KINEMATIC  MODEL 


The  kinematic  and  inverse  kinematic  equations 
describing  the  legs  of  the  walking  robot  are  developed  in 
this  chapter.   The  inverse  kinematic  equations  are  used  in 
the  simulation  program  to  compute  the  leg  joint  angles  as 
functions  of  the  end  positions  of  the  legs. 

In  order  to  avoid  writing  the  kinematics  independently 
for  six  legs,  an  imaginary  joint  (Joint  1)  was  placed  at  the 
center  of  the  body.   Rotations  about  the  Zo  axis  of  9t , j 
radians  corresponds  to  the  position  where  leg  j  is  attached 
to  the  body.   While  9i,j  is  shown  as  a  variable,  it  is 
actually  a  fixed  constant  for  each  leg. 

The  kinematic  model  of  the  walking  robot  is  shown  in 
Figure  1.1.   With  reference  to  Figure  1.1,  the  following 
definitions  describing  the  kinematic  model  are  made, 
Xo,  Yo,  Zo  is  the  coordinate  frame 

located  at  the  center  of  mass 
of  the  body 
Xi ,  j,    Yi ,  j  ,  Zx  ,  j        is  the  coordinate  system 

fixed  on  link  i+1, j  and 
centered  at  the  joint  between 
link  i, j  and  link  i+1, j 
9l>->  is  the  angular  displacement  of 


u,. 


Ej 


link  i,j  about  Zi-i,j 
is  the  distance  along  the 
Xi-i,j  axis,  from  the  origin 
of  the  Xi-x.j,  Yi-x,J; 
Z»-i,j  coordinate  system  to 
the  origin  of  the  Xi ,  j  ,  Yx  ,  j  , 
Zt , j  coordinate  system 
is  the  endpoint  of  leg  j 


9a,  j 


Xo   Uj.j 


Yx         .  Z* 


Xx  \m,J 


0L 


Y=  Y=, 

FIGURE  1.1   Kinematic  Model 


X=  \*,*   I  "x,   V,., 


FIGURE  1.2   Leg  Numbering  System 
The  numbering  system  used  for  the  legs  is  shown  in  Figure 


1.2.   The  view  is  looking  down  on  the  body  from  a  position 
along  the  positive  Zo  axis.   The  legs  are  spaced  evenly  at 
60°  intervals  with  leg  1  being  located  along  the  positive 
Xo  axis. 

The  development  of  the  kinematic  and  inverse  kinematic 
equations  follows  the  work  of  Paul  [1].   Homogeneous 
transformations  are  used  to  describe  the  coordinate  system 
of  link  n  with  respect  to  the  coordinate  system  of  link  n-1. 
This  transformation  is  called  an  A  matrix  and  is  referred  to 
as  Ar, . 

The  A  matrices  are, 
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where         C(9„,j)  =  cos(9n,j) 
and  S<9„,  j)  =  sin(9r,,  j) 

The  transformation   T«,    where, 
1.5  T»    =    AiA2A3A. 


describes  the  end  of  link  4  with  respect  to  the  body 

(^o.Yo.Zo)  coordinate  system.   Doing  this  multiplication 
yields, 

1.6     T»(l,l)  =  0(91^+6^^)0(83^+6,,^ 

T»(2,i)  =  S(ei,j+e*,j  )C(e3,J+9.,J ) 

t,(3,  i)  =  -s(e=(J+e»,  j) 

T.<4, 1)  =  0 

"Ml,  2)  =  -C(8i,j+6i,J)8(93,J+94,J) 

T»<2,2)  =  -S(e1,J+9i,J)S<93,J+6-.,  j) 

T»(3,2)  =  -C(9»,j+e«,j) 

T.<4,2)  =  0 

"Ml, 3)  =  -S<6x,j+ea,.i) 

T,<2,3)  =  C{ei.,j+da,4) 

T»(3,3)  =  0 

T.(4,3)  =  0 

T,(l,4)  =  C(Qx,A+@a,j)tl»,*C{Qm,*4Q+,*) 

+1»,jC(6b,ji)  +1=,  j]+1i,  j  C(6.,  j) 

T*<2,4)  =  S(6,,j+6i.j)(1,,jC(63,j+9,.j) 

*hm,  jC(8«,j)+lai,j]  +  lx,jS{9»,j) 

t»(3,4)  =  -u,  Js<e3,j+e»,1i)-i3,jS<e3,j) 

T»<4, 4)  =  l 


which   has   the   form, 

1.7             T.   = 

"a. 

a. 

A. 

EhO  .    J 
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0* 

A, 

Evo  i  J 

N. 

0. 

A, 

E«0  ,  J 

0 
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where  E„0,j,  Evo,j  and  E.o,j  specify  the  position  of  point 


Ej  in  the  Xo,Yo,Zo  coordinate  system. 

To  obtain  the  inverse  kinematics,  Equation  1.5  is 
premultiplied  by  Ai_1  using  the  form  of  TV  in  Equation 
1.7.  Equating  the  (1,4)  and  (2,4)  elements  of  the  resulting 
matrices  yields, 

1.8  C<ex,  J)E»o,J+S(6i,J)Evo,  j-1i,  j  = 

c(©»,J)[C(ft«,j+e.,j)i«J4+c<e«,j)is,j+ia,j] 

and, 

1.9  -s(ei,J)E»o,j+c(ei,J)Evo,J  = 

S(92,J)[C(93,j+84,J)l.,j+C(3,J)l3,i  +  l3,J] 
Solving   for   C(8a|J)    and   S(6a,j)    yields, 

1.10  C<6a(J)    =    [C(9i,J)&,o,j+S(9.,J)Eyo,j-l.,J] 

/[C(93, j+e,, j)l», ^+0(6=, j)l3, j+l»,  j] 
and, 

l.ii        s<e2jJ)  =  [-s(e.,J)EUo,J+c(6x,J)Evo,J] 

/[C(63, j+9», J)l»,J+C(93, j)13,j+1=,  j] 
Solving  for  9a,  j, 

1.12    e=)J  :  arotan{[C(e,,J)Evo,j-5(9i,J)E-c,,] 
/[C(ei,J)E«o,J+S(9.,  jJEvo.j-Ix,  j]} 

Equating  elements  (1,4)  and  (3,4)  of  IV  in  Equation 
1.6  to  the  corresponding  elements  of  T.»  in  Equation  1.7 
results  in, 

1.13  e^o.j  =  c(9x,j+e»,J)[c(e»,Ji+e»,j)u,J+c(©w,j) 

*l3,  j+la,  j]+C(9i,  j  )X»  ,j 
and, 

1.14  e.o,j  = -S(e3,j+e-.,4)u,  j-s(©»,  j )!»,,. 


Rearranging  and  squaring  Equation  1.13  yields, 

1.15  Kx.j3  =  [0(03,  j+e*,  j  )1»,  j+CfO^,  j  )  Is,  j  ]  = 
where,         Id  ,  _,   =  [E»o,  j  -C(8i  ,  j  +6=,  j  )  la,  j 

Multiplying  Equation  1. 14  by  -1  and  squaring  yields, 

1.16  E«o,j=  =  [S(63,  j+6*,  j  )l,,j+S<e3,j  )13,  j]  = 
Adding  Equations  1.15  and  1.18  and  simplifying  gives, 

1.17  Kx,  ,  =  +B.o,  j2  =  1»,  j2  +  13,  ja+2C<«.»,  j)l»,  jls.j 
Solving  for  G(8»,j), 

1.18  C(9«,J)  =  (Kt  ,  J=+E.c.,J  =  -l^,J  =  -l,,J2) 

/(2U.J1,,,) 
Then, 

1.19  S(8,,j]  =  sqrt[l-C=(e»,  j  )] 
and, 

1.20  e»,j  =  arctan[S(9*, j )/C(9», j )] 
To  obtain  8,,,,  the  multiplication, 

1.21  fe-'A=-'Ai-'T«  =  /W 

is  performed  using  the  form  of  T<  in  Equation  1.7.   Equating 
the  (1,4)  and  (2,4)  elements  of  the  resulting  matrices 
gives, 

1.22  C(e»)J)Ea,J+s(e3,J)(-g.o>Ji)  =  C(e»,4)u,j 

+1=,  J 

and, 

1.23  C(e,,J)(-E,o,J)ts(e,,J){-K=,,)  =  S(«JJ)i«,J 

where,         K=,j  =   C(3,,  ,+8=,  ,)E.<,,i+S(8,,  ,+9=,,  (E^o,, 

-c<e=,  j  )  1x^-1=,  j 


In   matrix   form,    Equations    1.22    and    1.23    become, 

1.24  [Ka.j  -E.o,/|  [0(6=^)1  = 
L-E.O.J            -K=,j    J  Ls(9.-3,j) 

Cramer's  rule  is  used  to  solve  for  S(e3)J)  and  C{fo,j). 
Dividing  S(93,j)  by  0(9*, j)  and  taking  the  arctangent  gives, 

1.25  e„,j  =  arctan{[S(e»,J  )1»,  ib,j 

+  (C(e»,  J)l,,J  +  i3,J)E.o,J] 

/[-(C(e,,J)i.,iti3,J)5=,J 

+S(9.,  j)1.,Xo,j]} 
This  completes  the  derivation  of  the  kinematic  and 
inverse  kinematic  equations.   Equations  from  this  chapter 
will  be  used  for  path  planning  and  for  deriving  the  dynamic 
equations  of  the  walking  robot. 
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II.  DYNAMIC  MODEL 

The  mathematical  model  describing  the  walking  robot  is 
developed  in  this  chapter.   The  mathematical  model  is  used 
in  the  simulation  program  to  determine  the  path  of  the  body 
and  the  legs  of  the  robot  when  torques  are  applied  at  each 
joint. 

The  nature  of  the  walking  robot  leads  to  nonlinear, 
highly  coupled,  and  quite  extensive  equations  of  motion.   In 
order  to  reduce  the  complexity  of  the  equations  and  of  the 
simulation,  the  following  assumptions  are  made. 

1.  The  body  is  heavy  and  slow  moving  compared  to  the 
legs. 

2.  The  body  maintains  a  near  level  attitude. 

3.  The  body  and  all  links  are  symmetric  about  their 
principle  axes. 

4.  The  body  and  all  links  are  made  of  homogenous 
material  with  no  concentrated  masses. 

5.  Three  legs  are  supporting  the  body  at  any  given 
time  (legs  2, 4, and  6  or  legs  1,3, and  5). 

6.  A  selective  surface  is  used  for  the  robot  to  walk 
on.   This  surface  has  the  following  properties: 

a.  When  it  is  time  to  switch  support  from  one 
set  of  legs  to  the  other,  the  level  of  the 
surface  is  the  same  as  the  level  of  the 
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endpoints  of  each  of  the  legs  that  are 
just  becoming  supporting  legs, 
b.  The  surface  never  interferes  with  the 
motion  of  the  non-supporting  legs. 
Assumption  1  allows  for  the  supporting  legs  to  be 
treated  as  massless  kinematic  linkages.   Assumptions  1  and  2 
allow  for  the  non-supporting  legs  to  be  treated  as  if  they 
were  on  an  inertial  reference  frame.   Assumptions  3  and  4 
reduce  the  calculations  needed  for  the  moments  of  inertia  of 
the  body  and  the  links.  Assumption  5  allows  for  instant- 
aneous transfer  of  weight  from  one  set  of  legs  to  the 
other.   Assumption  6  simplifies  the  simulation  program  in 
two  ways.   First,  6. a  allows  for  the  transfer  of  body  weight 
to  come  at  predetermined  times,  thus  doing  away  with  the 
necessity  of  monitoring  the  end  position  of  each  leg. 
Second,  6.b  eliminates  the  need  for  constraints  on  the 
possible  positions  of  the  non-supporting  legs.   While 
assumption  6  is  not  entirely  accurate  for  a  walking  robot, 
it  allows  the  simulation  program  to  concentrate  on  the 
dynamics  of  the  walking  robot  and  the  application  of  spline 
functions  to  guide  the  robot. 
2-1.   DYNAMICS  OF  BODY  AND  SUPPORTING  LEGS 

The  origin  of  the  body  coordinate  system  (Xo.Yo.Zo)  is 
located  in  inertial  world  coordinates  by  the  position  vector 
P, 
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where, 


P  = 


The  orientation  of  the  body  is  defined  by  the  Euler 
angles  <t>,  9,  and  ♦;  where  *  is  a  rotation  about  the  z  axis 
of  the  body,  9  is  a  rotation  about  the  new  x  axis,  and  +  is 
a  rotation  about  the  new  z  axis. 

In  writing  the  dynamics  for  the  body,  P„ ,  Pv,  p« ,  $, 
9,  +,  and  the  joint  angles  of  the  supporting  legs  are  the 
independent  generalized  coordinates.   This  gives  15 
generalized  coordinates  while  the  body  has  6  degrees  of 
freedom.   The  9  constraint  equations  come  from  holding  the 
endpoint,  Ej> ,  of  each  of  the  supporting  legs  at  a  fixed 
location  in  world  coordinates. 

Lagrange's  equations  are  used  to  write  the  dynamics  of 
the  walking  robot.   Defining  K  as  the  kinetic  energy  of  the 
system  and  P  as  the  potential  energy,  the  Lagrangian,  L,  is 
defined  as, 

2.1     L  =  K-P 
From  Greenwood  [3],  Lagrange's  equations  are  written  in  the 
form, 


2.2 


d_      (§L_)      - 
dt      I  3q»  I 

i    =    1,2,  ...  , 

15 
E   a»,iqi  +a™, 
i  =  l 


2L_   =      E  Xma™,i.+Ti  , 
9qi         m=l 

15    and, 

t    =0,      m=    1,2,. ..,9 
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where, 

^i  is  the  t**"  generalized  coordinate, 

X™  are  Lagrangian  multipliers, 

Ti  is  the  force  applied  at  the  itH 

generalized  coordinate, 

a-.i  =  SBm  , 
d<i± 

am,T  =  §3,*   , 

3t 
Bm  is  the  m*"  constraint  equation. 

The  generalized  coordinates  are  assigned  as  follows, 

Qi  =  P»  q&  =  t  qi  i  =  e3 ,  =, 

q=  =  Pv  q-7-  =  8z,P  qi3  =  9»,„ 

qs  =  P«  q=  =  63,0  qi.3  =  9z,r- 

q-»  =  *         q-»  =  6»,B      qx»  =  9s,  r- 

q=  =  9  qio  =  9=,„       qin  =  9»,.- 

with  p,  q,  and  r  being  the  supporting  legs. 

The  angular  velocities  about  the  principle  body  axes 

are  defined  as, 

tA.,o  =  angular  velocity  about  the  Xo  axis 
<jv,o  =  angular  velocity  about  the  Yo  axis 
<j«,o  -   angular  velocity  about  the  Zo  axis. 

From  Syman[2],  the  components  of  the  angular  velocity  vector 

are  written  as  functions  of  the  body  orientation  angles  as 

follows, 

2.3  u»,o  -   eC<+)+«S(6)S(+) 

2.4  wv,o  =  -eS(+)+*S(9)C(*) 
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2.5  u«  >0  =  ++<)>C(0) . 

The  kinetic  energy  of  the  body  is, 

2.6  K  =  (l/2)Mb(P„=B+Pyz+P,  =  ) 

+  (l/2)[I„,o(w„,o)  =  +  Iv,o(w,,,0)3 
+  1.  ,o(w.,o)=]. 
where, 

Mb  =  mass  of  the  body, 

I~ ,  o ,  Iv ,  o ,  and  I.  ,  o  are  the  moments  of 
inertia  about  the  Xo,  Yo,  and  Zo  axis. 
The  potential  energy  of  the  body  is, 

2.7  P  =  MbGPs 

where  G  is  the  acceleration  due  to  gravity. 
The  Lagrangian  is, 

2.8  L  =  (l/2)Mb(P„*H.fV*+Ps  =  -2GPs> 

+(l/2)[I„,o(w„,o)=+Iv,o(uv,o)3 
+  I.,o<w-,o):=] 
Taking  the  required  derivatives  of  L  gives, 


2. 

9 

d_ 
dt 

2. 

10 

dt 

2. 

11 

d_ 
dt 

2. 

12 

dt 

d_      (Mj\ 
dt      \9r\J 

\3PVI 

(at) 


=   MbP„ 


MbPv 


=  MbPs 

=   *[S:=!(9)S3(+)Im,o+S:=(9)C=('I')Iv,o 

+c»<e)Li,o]+6cs(e)C{+)S(+)L.,o 

-s<e)C(+)S(  +  )iy,o]++c<e)i«,o 
+2*e[C(e)S(e)s=(^)i„,o 
+C(e)S(9)c=(  +  )iv.,o-c(9)S(e)i,  ,•] 
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+2*+Cs*{e)c(+)s(+)(i»,o-iv,o)] 

+e=cc(e)C(+)S(4')(i-,0-iv,o)] 
+e+[S(6)(c=(  +  )-s=(+)  )(Im,0-iv,o) 
-s<e)i.,o] 

2.13  d_      (dL)      =  *S(9)C<  +  )S(+)(I»,o-Iv,o) 

dt    laei 

+e  [  c=  < + )  i- ,  o + s2  < * )  i  v ,  o  ] 

+<J>ec(e)C(*)S(i')(iM,o-iv,o) 

+« + S  ( 9 )  ( C=  ( +  )  - S=  ( * )  )  ( I„  ,  o  - 1  v ,  o  ) 

2.14  d_     f8Ll      =  *C{9)I«,0+*I-,o-<i»9S(9)I«,o 
dt     la*/ 


2.15  d_ 

dt 


f2L )     =  o 

1 393 , „  I 


2.16  d_      (9L ^      =0  V     J    =   P.l.r 

dt      V393,j/ 

2.17  d_     far. 


-) 


dt      U9 

2. 18  9_L_   =    0 
9P„ 

2. 19  9L_   -   0 
9PV 

2.  20  9_L_   =    -MbG 

3P« 

2.21  9L      =   0 
9f 

2.22  2L  =  *=C(9)S(9)[S=(+)I«,o+C3(  +  )Iy,o 
99 

-I-,o]+*8C(e)C(+}S{  +  )<L.Jo-Iv»o) 

2.23  9i  =  <i>=S=<9)C(  +  )S<  +  )<I..,o-Iv.o) 
9+ 

+09S(9)(C=(t)-S=(+)  ><r»,o-Iv,«»5 


16 


2.  24 


eL 


36* ,  j 

2.25 

3L 

303,  J 

2.26 

3L 

-ezc<  +  )S(  +  )(L.,o-ivo) 
=  o 

=  0 


39*» , . 

The  constraint  equations  are  developed  upon  the 
assumption  that  the  ends  of  the  supporting  legs  stay  at  a 
fixed  location  in  the  world  coordinate  system.   From  Chapter 
1,  the  position  of  Ej  in  the  Xo.Yo.Zo  system  is, 

2.27  E»o,j  =   C(ei,J+e=,J)[C(6.3,J+0»,J)l»,  j 

+c(e,,J)i»,J+ia,J]+c{el,J)i1,J 

2.28  Eyo.j  =  s(e1,J+e*,J)[C(63,  j+e^.jju.j 

+c(e3)  J)i*,i+ix,Ji+s(6-L,  j  )i.,  j 

2.29  E.0>J  =  -s(e3,J+e»,J)i*,J-s(e3,  j)i«,j 

The  position  of  Ej  in  world  coordinates  is, 
2.30 


where, 

2.31        Euler 

rotation 
and 
translation 

C(*)C(+)-S(*)C(9)S<+)  -C(*)S(+)-S(*)C(9)C(+)    S{*>S(G) 

S(<t>)C(*)+C(*)C(e)S(<t-)  -S<<t>)S(+)+C(>t>)C(9)C(  +  )  -C(*)S<9) 
S(9)S(+)                S(9)C(+)  C(0) 

0  0  0 

Performing  the  multiplication  in  Equation  2.30  gives, 


B»WlJ 

Euler 

EmO,  j 

Eyw  ,  J 

^ 

rotation 

EyO  ,  J 

Ekw  ,  J 

and 

Ek  O  ,  J 

1 

translation 

1 
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2.32  Ek„,j  =  c<<t>)C(++ei,j+e3,j  )[C(9r,, *+e»,j)i»,j 

-s<0)C(6)S(++ex, J+e=, j ) 

+  0(63^)13^+12,^] 

-S<<0)C(9)S(  +  +ex,  jjlt,  j 

-S(*)S(e)[s<e=,,J+«.,J)u,J 

2.33  eVw,j  =  s(*)c<++ex,j+ea,j )[C(es,j+e», j)i»,j 

+c<63, jjis.j+ii, j]+s<*)C(++ei, jjix.j 
+C(*)C(e)S(++e1(J+e=, j) 

*[C(63,J+9»,J)U,  j 

+c<*)C(e)S(++ei,J)i1, J 

+c(«)s(8)[S(e»,J+«„J5u,J 

+s<e»,J«)i»,J,3+pv 

2.34  e«w,j  =  s(9)S(++ex,J+e3,J)[C(e3,J+e»,J  )i»,  j 

+c<e3, j)i3,j+ia,j]+s(9)S{++ei, j)ii,j 
-c(9)[S(e3,j+e«,  j)u,  j  +s<e=,  jHs,  .,  ] 
+p« 

The  constraint  equations,  which  are  of  the  form, 

2.35  Bm  =  0, 

are  written  in  terms  of  the  support  legs  p,  q,  and  r. 

2.36  Bx  =  [C('t>)C(  +  +ex,F,+e=<0) 

-s(*)C(0)S(<i'+ex,„+9=,B)] 
*[C(e3,„+9.,«,)i,,R+c(e3,F,)i3,„+i=,P] 
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+c(»>e<*+e»,„)ii,„ 

-s(-t>)[C(e)S(>i'+e1,„)i1,(, 

+S(9)S(93,„+8., „)]>,,„ 
+S(9)S<93,„)l3,„]+p„-E„..,„ 

2.37  Ba    =    [C(-0)C('t-+e1)«,+92,^) 

-s<<i))C(e)S(*+ex,fl+e=,,)] 

+c(4>)c<*+e».«,)u,«, 

-s(<t>)[c(e)S(++9x,^)i1|e, 

+S(9  )S(93,  „ )  i3, «,]  +P„  -&.«, ,  , 

2.38  B3   =    [C(<l>)C(  +  +ei,,.+e=,.-) 

-S(*)C(9)S(  +  +8x,r-+93,^)] 
*[C(9=,^+8»,^)1»,^+C<93,^)l3,^  +  13,^] 
+C(*)C(  +  +91,^)11,^ 

+S(9)S(93,r.+9.,^)U,^ 
+S(9)S(93)^)i3,^]+pM_E><W;^ 

2.39  B*   =    [S(<K)C(++ex,p-f«a,R) 

+C(4>)C(e)S(  +  +8x  ,„+«=,„)] 

+S(9)S(93,„+0«,„)1„JB 
+S<9)S(93,F,)l3,(,]+pv. -£*.„,„ 

2.40  BH    =    [S(*)C(  +  4e1, «,+©»,,) 
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+C(<t.)C(0)S(*+e1,^+e3ja)] 

*[C(83,c,+e*,c,)u,a+c(e3,,)u,^+i=,«,] 

+S(*)C(*+ei,«,)i1,^ 

+C(*)[C(e)8(++e»,„}i*,, 

+S(9  )  S(93, «,)  1=»,  c] +Fv-E,.w,  „ 

2.41  Ba    =    [S(<D)C(*+9x,^+9=,,-) 

*[C(93,^+e*,r-)l*,,-+C{  9=,,-)  lm.r+la.r] 

+s(#)C(++e»,^)i»^ 
+c(*>cc(e)S{++e»jr.)it,r. 

+S(9)S(9=,^+0,,^)Ujr. 
+S(9)S(e=s,^)l3,r.]+Pv-Ev~,^ 

2.42  B^    =   S(9)S(  +  +9x,P+ea,„) 

*[C(8s, »+&»,,.  }l«,»+C<9<»,,.)l«,,.+la„.] 

+S(9)S(  +  +91,„)U,R-C(9)S(93,P+9,,„)UJ 
-C(9)S(9=,„)l3,p+P,-E«w,0 

2.43  Bb    =   S(9)S(  +  +9i,<,+9=,„) 

*[C(93,,+8.,c)l.,,+C(93,a)l3,a  +  l=,.,] 
+S(9)S(  +  +9i,q)l1,«,-C(9)S(93,a+9»,c)l», 
-C(9)S(93,^)l3,,+P«-E.w,«, 

2.44  B,    s    S(9)S(  +  +9ljr-+8=,^) 

*[C(93,^+9»,^)U,^+C(93,^)l3,,-  +  l=,r-] 

+S(e)S(++ex,r-)ii,^-c(e)S(93,^+e»,,-)i», 

-C(9)S(93,^)l3,^+P,  -E-~,r- 

The  an,,t    are  derived   from  the   constraint   equations. 
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2.45 


a! 

,»=1 

a. 

,a    =  0 

a* 

,3=0 

ai 

,*  =  c 

c-s(<t>)C(++el,c,+e3,F,) 

-c(t>)c<9)S(++e1,„+ez,„)] 

*CC(e=,„+9»,„)i..,„+c(e3,«,)i3,„+i=,„] 

-s(<t>)C(++e1>0)i1,„ 

-C(*)C(e)S(++e1,„)i1,„ 

-c(*)s<e)S(e3,„+e,,P)u,[. 

»i,»    =   S(4>)S(9)S('t'+ei,„+82,B) 

*[C(e3,„+e*,B)i»,„+c(93,„)i3,B)+i=:,P] 
-s(*)c<e)s<e3,e,+e.,„)i-,„ 

-S(<t>)C(9)S(63,„)l3,„ 

ax,«,  =  [-C(<t>)S(++ei,„+ea-,p,) 

-s(«i>)C(8)c<++e»., „+«■,.)] 

*[C(9»,^+e»,l.)u,1.+c(e»,»)i*l„+i»)W] 

-C(*)S{*+e*„.)u,„ 

-so>)C(9)c<*+exJO)ix,„ 
ax,,  =  [-c(«)S(++ex,[,+e2,«.) 

-s<*)c<e)C(*+e,, „+««,„}] 

*[C(e3,R+9*,„)i»,F,+c(e3,„)i3,0  +  i2,P] 
ax, B  =  [c<*)C(++ei,„+e3!,«,) 

-8<<>)C(8)S(++«», ,+«»„,)] 
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ai,,    =   C-C(«)C(  +  +6l,1»+9z,«.) 

+s<<i>)c<e)s<++9x,„+e=!,„n 

*s(93,„+e,,„)u,„ 

-s(<t>)s(9)C(e3,„+e.,B)i»,„ 

al;io  =   0 

ai,n  =0 

a.i,  i=  =0 

ai,i3  =0 

a1;1.»  =  0 

ai,ia     =:    0 

ai,t    =0 

az,  i=l 

az,i    =   0 

a=,3    =   0 

aa,*    =    [-S(*)C(*+ei,«1+e*,.,) 

-C(*)S(8)S(9,,qtfl.,q)l.,q 

a=J=  =  s<t>)S(e)S('i'+ei,,+e=,q) 
+sct>)S(e)S(++el,fl)i1,c, 
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-s(*)C<*>8<e»J„+e., „)!.»,., 

-s<o>)c<e)s<e3,  ,)i»,„ 
az,6  =  [-C(«)S(++e1,q+e3,.,) 

-s(«t>)C(e)c<++e1,«,+e=!,,)] 

-c<«)S(*+e», .)!!,«, 

-S(*)C(e)C(i-+ex,c,)i1,«, 

a=,T    =  0 

a=,o    =    0 
a=,„    =  0 

a=,io  =  c-c(<i>)S('i'+e1,„+e3!,„) 

a=,xi  =  cc(<t>)C(++e1,«,+e=,«1) 

-s(*)C(e)S(++e1,a+0a.,^)] 

-s(»)S(e)[C(9,,qte.,q)i,,„ 
+c<e3,  „)i3,«,] 
te,i2  =  [-c(*)C(t+ei,,+e=,a) 

+s(<t>)c<e)s<++e1,c,+e=,„)] 

aa,  13    =    0 
a», i»    =   0 

92,19      =     0 

a=,t    =  0 
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^,1=1 

a=*,=    =  0 

as.s   =  0 

-c<*)C(e)s<++6x,^+e=,^)] 
-c<*)c<e)S(++ex,^)ix,^ 

-C(<t>)S(e)S(93,r-+9»,^)l»,^ 

-c(<t>)S(e)s<e3,^)i»- 
a=.,=  =  s(«)S(e)S(++ei,^+e3,r-) 

*[C(6=,,  ^ +9*.  ^)l»,^+C(93,^)  13,^  +  1=,^] 

+s<<D)s<e)s<++e1,^)i1,.- 
-s(<t>)C(9)S(e=s,^+e,,^)i»,^ 
-s<*)C(e)S(e3,^)i=,,^ 
to,*  =  [-c(<t>)S(++ei,,-+e:=,,-) 

-s(«t>)C(e)C<++ex,^+e=,^)] 
*tc(e»,,-4«*„.)l*,,-+c<9*,r-n»,r-+l»,.-] 

-C<0>)S(  +  +9x,^)ll,^ 

-s(<*)C(e)C(++91,^)i1,r- 

as,7  =  0 
to,o  =  0 
to,f     =    0 

&3,   19       =      0 

ta,u      =0 

a=,1=    =   0 
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a3,.3  =  [-c(*)S(4'+ei,^+e=,,-) 

-S<<t>)C<e)C<  +  +6x^+6=,^)] 

*[C(e3,,-+9»,,-u»,.-+c<e»,-H!,,.-+i»,.-] 

as,,!-,    =   [C(«l>)C(  +  +ex,^+6=,,-) 

-s(<t-)C(e)S(*+e1,^+e3!,^)] 
*c-s<e3,^+e*,^)i*,^-s(  e»,.-)i»,,-] 
-s<*)S<e>  [C(e„„. +«.„.)  u,- 

as,..,  =  c-c(<t>)C(++ei,.-+e*,^) 

+S(*)C(6)S(++9x,^+e3,^)] 
*S(83.,  ^+9^,^)1,,^ 

-S{*)s<e)c<e.,,-*e»„.}i*,»- 

a^.t  =  0 
a.,x  =  0 
a»,=  =  1 
a., 3  =  0 
a*,*    =    [C(«)C(++9x,p+e3,P) 

-s<*)c<e)s<++e»„. +«*,„)] 

*[C(93,r,+e»,„)i.,„+c(e3,„)i3,P+i=,„] 
+c(*)C(  +  +e1,c,)i1,„ 
-s(*)c<e)S(++e*,„u»,« 

-S(*)S(9)S(83,„+9»,„)1.,„ 

-s(*)S(e)s<e,,B)i»,,. 

a.,0  =  -c<«)S(e)S(++ex,P+e3,„) 

-C(*)S<9)S(++ex, „)!,,„ 


2  5 


+C(*)C(9)S(63,r,+e,,  „)l*,„ 

+c(<t>)C(e)S(e»,„)i;s,0 
a*,*  =  [-S(*)S(++ex,„+e3;(,) 

+c(*)c<e)C(++ex,„+e=,„)] 

-s<*)S(*+el,P)i1,t, 
+c(*>c<e)C(++e1,„)i1,„ 
a*, -,  -  [-sct>)s(++el,„+e3,P,) 

+c(<t>)C(9)C(++et,„+e=,F,)] 

a»,„    =    [S(<t>)C(++9i,«,+9z>B) 

+C<0)C(9)S(  +  +9x,„+e=,P)] 
*[-S<93,„+e»,„)l«,„-S(93,„)l3,P] 

+C(*)S<9)C(93,„)L,,„ 

a,,,  =  -[S(<oc(++ei,B+ea,„) 

+C(<t>)c<e)s<++e1,„+ez,„)] 

+c(*)S(e)C(e=,,p+9.,„)u,„ 

a», io=0 
a»,  n=0 
a,, 12  =  0 
a», 13  =  0 
a*, x*  =  0 
a», is  =  0 
a-»,t    =   0 
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a«,i  =  0 

a«,=  =    1 

a=,3  =   0 

a=,»  =    [C<<t>)C(++e..,.,+9=,„) 

-S(«>)C<e}S<++0»,,+e,,„)] 

*[c<93,,+e«,,)i,,fl+c(e3,,)i3,«,+ia,M] 
+C(<t>)C(++ei,q)ix,, 
-s<t>)c<e)S{++e1,^)i1,e, 
-s<<t>)S(e)S(93,,+e»,I1)i,,a 

a=,H  =  -c(«)s<e)S(++6i,<,+e3,^) 

-co>)S(9)S(++el,a)i1>c, 
+c<«>)c<e)s<e»,«,+e»,„}i*,* 

+c<«)c<e)C(++©»,  «,+e»,a)] 

-s<*)S(++ell,)i1,«, 
■<-c(*)C(e)C(++91, „)].!,„ 

a«j,T  =  0 
ao,«  =  0 
a=,,    =   0 

+C(*)C(e)C(*+©», ,+«,,„)] 
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^.n  =  [S(*)C(++ei,„+e=,«,> 

+c(*)C(e)S(++e1,^+e=c,<,)] 

*[-S(83,q+94,,)U>q-S(93,q)l,,Q] 

+C(<t>)S(e)C(9=,„+e.,c)i»,«, 
+C(o>)S(e)C(e=,,«,)i3,e, 
a»,i=  =  -[s(«)C(++e.,q,+e=>a) 

+c<*)C(e)S(++e1, „+©=,.,)] 
*S(9», •,+©•, ,,)U,«, 
+c(*)S(8)c<e=,^+e-,,)u,q 

a>,i3    =   0 

an, i»    =   0 

ao,  is    =   0 

8ta,t     s    0 

a*,,  i=0 

a«,,3    =   1 

a*, 3    =  0 

a*,,*    =    [C(«)C(*+ei,r-+9=,r- ) 

-s<*)C(9)S(++ex,^)ix,^ 

a«,,=  =  -c(«)s<9)S(++e1,^+e3,^) 
-C(«i>)S{e)s<++e»J^)ii,^ 
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+c(o>)C(9)S(e3>^+e.,r-)u,r- 

+C(4>)C(9)S(93, ,-)].=,,.- 
a*,,*    =    [-S{<t>)S(  +  +6i,^+eiJr-) 

+C(<|))C<9)C(  +  +91,r.+e=,^)] 

+c<i>)C(e)C(++9x,^)i1,^ 

a«,r    =   0 
a*,=    =   0 
a*,,    =   0 
a*,,  io    =   0 
ae,,  ii     =0 
a»,  iz    :    0 

a*, 13  =  c-s(<<>)S(++ei,,-+e2,.-) 

+C(«t>)C(e)C(++ei,^+e=.,r-)] 

as.,1,  =  [s<*)C(++ei,,-+e»,.-) 

+C(*)C(9)S(  +  +e,,r-+9=,^)] 

+c(*)S(e>c(©», «.+©»,-)  u, - 

+C<<t>)S(9)C(93,^)l3,r- 
a*,i=    =   -[S(«)C(  +  +9i,^+9z,r-) 

+C(*)C(9)S(++ei,^+9=,^)] 
*S(9=,.-+9.»,,-)l,,,I- 

+c<«)S(e)c<es,^+e»,^)u>r. 

a«,,t    =  0 
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a^,i  =  0 

a-^.z  =  0 

a-T,=.  =  1 

&.-?,*  -  0 

a^,=  =  C(9)S<++9i,„+9=,P) 

*[C(e3,„+e.,r,)i»,„+c(e=5,„)i3,„+i: 

+c(6)S(++e1,„)ix,^+s(e)s(e3,„+e«, 

+s(e)S(9,,p)i=,P, 
a^,A  =  s(9)C(+-+9i  !f>+e=,P) 

*CC(e=,„+e*,p)i,,„+c(9=,„)i=,,B  +  i= 

+s(6)C(++e1,„)i1,p 
a-,,-,  =  s(9)C(++9i,„+e=,„) 

a^.o  =  s(9)S(-i'+e.,F,+e=,p) 

*[-S(93>„+e.,p)U,0-S(93,„)l3,«.] 
-C(9)C(93,(,+9.,B)l.,p-C(9)C(93,P: 
a-,,,.    =   -S(9)S(++9i,«,+93,„)S(93,„+9»,„)l. 
-C(9)C<93,„+e»,„)l,,p 

Sr.io  =    0 

ar.n  =0 

a-r-,  i=  =   0 

a?-,  13  =   0 

37,14       =      0 

a7,ig    =   0 
a-»-,t    =   0 

ao,i    =0 


,,.] 


,»] 


1=,, 
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a«, 2  =  0 
aa,3  =  1 
as,4    =   0 

ar,,„  =  c(e)S(>('+ei,,+e2,c) 

*cc(93,,+e.,„)i,,«,+c(e3,,)i3,.,+i=,<,] 

+c(e)S(++ei,»)ii,.,+s(e)S(9.-3,fl+©»,a)u, 

+S(9)S(e3,,)i3,, 
as,a  =  s<e)C(++9i,«,+e3,a) 

*CC(e3,«,+e»,fl)i»,^+c(65i,^)i3,q+ia,^] 

+s<e)C(++e1,,)i1,cl 

Sa,7  =  0 
aa,a     =    0 

a=,,    =  0 

a=,io  =  s<9)C(++ei,«,+e3,^) 

a«,i.    =   S(9)S(++ei..a+92,.,) 

*[-S(eS|,+6.,q)U,,-5(e3,q)lSiq] 

-c<9)C(e3jcl+e»ia)i4,^-c{9)C(e=.,c,)i-3,0 
a»,i=  =  -s(9)S(*+ei,,+ei,q)S(e3,»+e.,a)i.,I, 
-c(9)C(e3,q,+e»J„)u,, 

aa,  13     =    0 
te.i.     =    0 
aa,m     =    0 
a=,t    =   0 
a-» ,  ,    =0 
a-v.  2    =0 
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*W,=  *  1 
a,,„  =  0 
a-r.o    =  C<e)S(++9x,r-+9=,^) 

*[C(93,rt9,,r)l,,.+C(e,,.)l3,r  +  lS,r] 

+c<e)s<++e1,^)i1,^+s(e)S(9.3,^+e»,^)i., 

+S(6)S(93,^)l=.,^ 

a.,,*  =  s(9)C(++ex,^+e3)^) 

a-r,T    =   0 

a.-,,  a    =   0 

a-»,,    =  0 

a>*,xo    =  0 

a-, ,  n    =0 

a-r,  12    =    0 

a-»,x=    =   S(9)C(  +  +9x,^+8=,r-) 

*[C(0»,  r-+e*,r-)l*,r-+C(e<.,^)  Is,  r  +  lm,  »•  ] 

a-,,x»    =    S(9)S(++9i  ,,-+9=,,-) 

*[-S(9=,^+9»,^)U,^-S<9=,^)l3,^] 
-0(9 )  0(9=,,- +9»,.-)L»,.- -0(9)  0(9:3,  .-H*,  r 

a^,x„     =     -S<9)S(  +  +9x,.-+93, r-)S(9:3, .-+9., .-CU, - 

-0(9)0(93,^+9-.,^)  U,.- 

a..,*    =0 

The  complete   set   of  Lagrange's   equations   are, 

.      9 
2.46  MbP„-S;   Xma™,x    =   0 

m=l 
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9 
2.47  MbPv-E  Xma-,2    =  0 

m=l 


2.48  MbP„+MbG-E  Ama™,3 

m=l 


2.49  *[S2(9)(S3('t')lK.,o+C=('l')Iy,0)+C=(e)I«,o] 

+es(e)C(+)S(+)(i^,o-iv.,^) 

++c(e)i«,0 

+2*ec(8)S(e)[S3(+)i»,0+ca!(  +  )iv,o-i«>o] 

+2*+S=(6)C(*)S(+)(I-,o-IVlo) 

+eaC(e)C(+)S(+)(L.,o-iv,o) 

4&*Sft)[(G»(  +  )-S"{+))<L..«-X*.o)-X-.*] 

9 

~L     Am  3m,  4      =     0 

m=l 

2.50  *S(e)C(  +  )S(+)(I»,0-ly,0) 
+6  <  C=  ( +  )  U  ,  o +S=  ( *  )  Iv ,  o ) 

+*+s(e)[(Cp'(  +  )-s=(  +  ))(i„,o-iv,o)+i«,o] 
-2e+C(+)S(+)(i«,0-i»-,o) 

-<t>=c(e)S(e)[s=(<(')i-,0+c=!(t)iv..o-i«,o] 

9 

—  £*      Am  dm  j  a       —      0 

m=l 

2.51  *C(6)I.,0++I«,0-«=S:!!(e)C(  +  )S(  +  )(I-,o-Iv,o) 

-*es(6)[(c=(>t.)-s3(  +  )(i.,o-iv,o)+i. ,«,] 
+e=c(4')S(  +  )(i„,o-iv,o) 


tj     Am  3m  ,  <s      —     U 

m=l 


3  3 


2.52     -E  X„a„,^  =  f7 
m=l 


2.  53     -E  ^a.,s  =  T= 
m=l 


2.  54    -E  X»am,-r  =  TV 
m=l 


2.55     — E  Am  a™ , i o  —  Tio 
m=l 


2.  56    -E  X™a».,  u  =  Tu 
m=l 


2.57    -E  X«a™,i=  =  Tiz 
m=l 


2.  58    -E  Xma.,  »=  =  Tx= 
m=l 


2.  59    -E  X™a„,x.  =  Tx- 

m=l 


2.60  -E  ^,a.,10  =  T.b 
m=l 

15 

2.61  E  ai,»qi+ax,*  -   0 
1=1 

15 

2.62  E  a=,iqi+a3,*  =  0 
1=1 

15 

2.63  E  as.njiias,*  =  0 
i  =  l 

15 

2.64  E  a.,i4i+a«,t  =  0 
i  =  l 


34 


15 

2.65  Z   a=,i4i+a»,*  =  0 
1=1 

15 

2.66  Z   a*.,iqt+a«.,*  =  0 

i  =  l 

15 

2.67  Z  a.*,  ±  At. +a.T , *.    =  0 

i  =  l 

15 

2.68  Z   aa,  •  qi  +a», *  =  0 
i=l 

15 

2.69  Z   a^,>4i+aT,<:  =  0 
i=l 


where, 

TV  is  the  torque  forcing  9=,«> 

T»  is  the  torque  forcing  9-3 ,» 

IV  is  the  torque  forcing  9»,p 

Tio  is  the  torque  forcing  ©=,<, 

Tix  is  the  torque  forcing  9*,., 

Ti=  is  the  torque  forcing  9»,<, 

Txs  is  the  torque  forcing  9=,r- 

Ti»  is  the  torque  forcing  93,,- 

Tio  is  the  torque  forcing  9»,r- 

Rearranging  Equations  2.52  thru  2.60  and  leaving  out 
the  zero  terms  yields 


2.70 


ai  ,  -r     a-»,  -r     &r  ,T 

Si ,  a  a* ,  a  a-r ,  o 
ax  ,  ^  &4|T  a-y,  ^ 


Xi 

-TV 

A* 

= 

-T= 

Kt 

-T-. 

- 

3S 


2.71 


2.72 


az  i  x  o  Qja  ,  x  o  a»  ,  1  o 

az  , 1 1  ao  .  x  x  as .  x  x 

a3  ,  x  z  Ob  i  x  2  ELa  ,  x  z 

SLs  |  x  3  SLa  ,  X  3  a-r  j  X  3 

fis  ,  x  *  9a  ,  X  *  £L^  ,  x  * 

^3  |  X  B  &A  ,    X  B  a-T  ,  X  B 


>Z 

Xo 

x= 
x* 

X-P 


-Txo 
-Txx 

-Txz 

-Tx, 
-Tx* 

-Tib 


Solving  for  the  X's  gives, 


2.73 


X 


[-TV  ( a*  ,  aa-7- ,  -*-a*  ,  ^a^ ,«) 

+Tb(  a*,  7a7,T-a*,  "ra^,  -r ) 

-T-,  (  a* ,  -,&■* ,  B-a* ,  «ar ,  ^ )  ]  /DEN1 

X*  =  [T7(ai,aa7,»-ai,»a7,a) 
-To(  ax  ,  -T-a-7-,  ■•—ax  ,  ^&r ,  -r ) 
+T^(  ax  ,  -,&■?,  B-ax  ,  9a7,  -,  )  ]  /DEN1 

^    =    [-T7(ax,oa»,-»-ai,-ra»,o) 
+Tb(  ax  ,  *-a»,^-ai  ,7a«,7) 
-T.p.<  ai  ,7&.,  o-ax  ,  Ba.,  -,  )  ]  /DEN1 

Xa    =     [-Tio{  as  ,  x  x  Qa  ,  iz  — as  ,  xzdjs  ,  11  ) 

+Txx  ( ao,  ioara ..  xz— a»,  xzaa,  to) 
-Txz(aB,  K>ai,  n-ao,  nan,  n>)  ]/DEN2 
ab    —    [Tio(  az ,  i x aa ,  xz-az,  xza» ..  x  x  ) 

-  Tx  x  (  az  ,  io3h;  xz  — az  ,  xz£b  ,  io] 

+Txz  (az,  loss,  xx-az,xxaB,xo)]  /DEN2 
a»   =    [-Txo(  az,  xx as,  xz-az,  i  =  ao,  n  > 
+Tx  x  ( az ,  xoas ,  xz— az ,  x zaa,  xo ) 
-Txz(az,xoao,  xx-az,  naa,  xo)]/DEN2 
'V;s    =    [-Ti3(aA,i4a7,io-a6,iaa7,i4] 
+Tx*  ( a& ,  xsa-r ,  xo— a±. ,  id&t ,  it) 
-Txb  (a*.,  x=a-»,  x»-a«,,  x-»a-»,  is]  ]/DEN3 
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where, 


^*»   =    [Ti3{a3,  i4a»,io-a3,ioa-»,i4) 

—  Tl^.  (  £|3  ,   XSdTj    18- St3  j    10&T,    1-3  ) 

+Tia(a3,  13^,  14-as,  i-»a-»,  X3>]/DEN3 

A^     =      [—  Ti3  (  a=s  ,  148a  ,  ia  — as,  isd6  ,14) 
+  Ti*  (  33  ,   13&6,    1B-S3,   laSLh,    13  ) 

~Tio  ( &s ,  isaci,  i4~as ,  i4^>,  ta )  ]  /DEN 3 

DEN1    =   ai>T(a*,oa7,7-aA,^a7Jo) 
-a-*,-7'{  ai  ,oa-z,  -»-ai ,  ^a?-,  a) 
+S-7', t (  ai ,  sa4 ,  ^— ai ,  va4,  o ) 

DEN2    =   aa,  io  (  an,  i  ia»,  i=-aa,  lzao,  1 1  ) 
~ao,  io  ( aa ,  1 1 as,  i3— az ,  128s,  11) 
+a»  j  10  ( a=s ,  1 1  aa,  13— a= ,  12 as,  11) 

DEN3    =   as,  i3(  a6,  i4a<r,  ia-a6,  loaT,  14  ) 
-a&,  13 { a» ,  14a* ,  10-83 ,  lodv,  i« ) 
+a«? ,  13  { a3 ,  i-*a&,  10—33 ,  iaa&,  14) 
Writing   Equations    2.46    thru   2.51    in  matrix    form  gives, 
2.74 


M 


"  P-" 

_Gx" 

~Qx" 

"0" 

Pv 

G= 

su 

0 

p. 

+ 

Os 

+ 

©3 

= 

0 

* 

G» 

Q» 

0 

9 

G„ 

* 

0 

+ 

G«. 

Q*. 

0 

where  M  is  the  6  by  6  mass  matrix. 

The  non  zero  elements  of  M  are, 
2.  75     i,,  ,  =  Mb 
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m=,=  =  Mb 
B=,3  -   Mb 

m»,*  =  s=(e)[s3(  +  )i-,o+c2(  +  )iy,o]+c3(e)i.,o 

m»,H  =  S(e)C(  +  )S(+)(L,,o-Iv.o) 

m»,«.  =  C(0)I.,o 

IDs; *  =  nw  i  a 

Bb.m  =  C2(  +  )IM,o+S=!(  +  )Iv,c» 

fflo.»  =  !&»>«» 

m*. .  «.  =  la  ,  O 

The  elements  of  the  G  and  Q  vectors  are, 
2.76     Q*  s  0 
G=  =  0 

G3  =  0 

GU  =  2*8C(e)s<e)[S=(  +  )i»,o+c=!(  +  )iy,o-i.,o] 
+2-i+s2(e)C(>t')S(  +  )(i-,o-iv,D) 
+e=c(6)C(+)S(  +  )(i«,o-iv,o) 
+e+s<e)[(c=<  +  )-s=(+))(i»,o-iy,o)-i«,o] 

g„  =  --i=:c(e)S(e)cs3<'t')i»Jo+c=('i')iv,o-i.,o] 

+<t>+S(9)[(C=<<l.)-S=<<l<)  )(I^,o-Iv,o)+I«,o] 

-2e*c(+)S(+)(i»,o-iv,o) 

g*  =  -<i>=s=!(e)C(')')s('(')(i»,o-iv,o) 

-*es<e )  [  <  c=  ( +  >  -s=  (+ ) )  <  u ,  o-iy ,  o )  +i« ,  0] 
+e»C(+)S(+>{L.,o-iv,<») 


2.77 


L      Ant  Sim  ,    : 

m=l 
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ra=l 


Q=*  =  MbG-E  ;wa™,3 
»=1 


ra=l 


«CB   —   —  L   Am  3m  j 

m=l 


m=l 

Solving  Equation  2.74  for  the  second  derivative  terms 


gives, 


2.78    P„ 
Pv 

p. 
* 

9 

The  non-zero  elements  of  M-1  are, 

m_1l,x   =   1/ffl!,! 


[~Qi~ 

"Gxl 

Q= 

G= 

Q3 

+ 

G-3 

0. 

G» 

0. 

Go 

Lg*. 

_G*J 

SB-1. 


3  =  l/mi , 1 

3  =  l/ffii , 1 

»  =  m=i,am«,,e,/DET 

a  =  -m»,omo,^/DET 

■=■  =  -m»,*mB,  =  /DET 

■»  =  m~  *  ■» ,  = 
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( m» ,  »m=. ,  ■=.  -m» ,  *= )  /DET 
iiw,=ib«,o/DET 

( ro» .  ■»  m=  ,  h  -m« ,  a= )  /de  T 


where, 


det  -  at.,»B)B)anie.,«i-m»,a=iB«i,«.-!a«,<.;zn»a,i!> 

The  state  variables  are  defined  as, 

2.80  y»  -  P„  y„  =  e  y*  =  P. 
y=  =  Py  y«,  =  *  y«.o  =  ♦ 
y3  =  p«        yv  =  Ph        yn  =  e 

y»  =  <t>  y=  =  Pv  yi=  =  + 

The  state   equations   for  the  body   in   final    form  are, 

2.81  y,    =   y, 
y=  =  ya 
y=   =  y-r 
y«  =  yio 
y=  =  yn 
y«.  =  yu 

y^-  =  (  -Qt  -Gi  )  /nu ,  x 
y=  =  (-Q^-Gaj/mi,  x 
y-»  =  (-fti-G^J/mi,  i 
yi°  =  CiB=,airk.,o(-Q«-G»)-m»Jam«.,d.(-Qis-Ga) 

-»• ,  *BtB .  =  (  -Qo  -G<* )  ]  /DET 

yu  =  [ -m» ,  =nk> , «.  ( -Q»-G») 


40 


+m* ,  aifr* ,  *  {  -Qft -G*  )  ]  /DET 
y«  =  C-m*,<.D*3Jo(-Q*-G-»)+nw,BiiwJ*<-Q»-G«) 

Rearranging  Equations  2.61  thru  2.69  and  leaving  out 
the  zero  terms  gives, 


2.82 


2.83 


2.84 


where, 


ax  , t  ai  ,a      ai,^ 

8l*,^  a* ,  a      a* ,  -9 

a?- ,  -t  a-r- ,  a      a-7- ,  -r 

a=,  io  a=a  i  1 1      a=c  i  xz 

&«s*  io  ae>  j  1 1        aa  i  iz 

a«,  xo  as  ,  x  x         ae>  ,  12 

3^,13  &S,  14          &3rie> 

&«•,  13  £!&  ,  14         &A  )  xa 

*» ,  i  3  a^  .14       a-r ,  X  B 


©3  ,  fs 

*Ci' 

03  ,  o 

= 

c» 

04  |  p» 

c^ 

Cx  =  -(P«+ai  ,»*+ai  ,a6+ai  ,a+) 
Ca>  =  -<P„+a=,.»<i>+a=,ae+a=, 
C=  =  -(P*.  +a3,»*+a3,=0+a3, 
C-»  =  -{Pv+a*,»<i)+a»,ne+a>.,«.+  ) 
Co  =  -(Pv+an,»*+aa,ae+aB  ,«.+  ) 
C*.  =  -<Pv+a«.,»'l)+ae.,ae+ae.,«.'i') 
Ct  =  -{P.+a-T-.Be+aT,*.*) 
C=  =  -(P«+a=..=e+a=,«.>i') 
c-»  =  -(P-+aT,a6+a-»,**) 
Solving  for  the  8,^  terms  gives, 
2.85     ez,„  =  [Cx  (a»,=a-r,-»-a.,»a^,  =  ) 
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©3  ,  f> 


9* ,  p    — 


63,, 


-c. 

+C7 
[-c 
+c« 

-Or 

-c. 
cc* 

-Co 

+c„ 

[-G 

+CD 

-c. 
tc= 
-c= 

+Cb 

[G» 

-c 

+c, 

[-C. 
+C* 

-c, 

[Co 

-c* 


ax  ,  m&.-7 ,  T-ax  j  Td7 ,  ts ) 
ai,aa4,f-ai  ,-*&*,«)  1/DEN1 
( a-» ,  -r  a^ ,  ^  ~  a* ,  ^  a?- ,  -*■ ) 
a*  ,  -7-a-r ,  f-ai ,  -ra-^,-7- ) 
ai.Ta^^f-ai.faAjT )]/DENl 
a* ,  tQl-7  ,  s-&*  joa-T- ,  -r  ) 
ax ,  787,  a-a*  .oa?,? ) 
a*,  7  a*,  s -ax  ,004,7)  3/DEN1 
as  1 1 1  as,  12-aa  ,  1280 ,11.) 

;i2  .    I   )  &B,    12-82,    12'lB,    XX    ) 

a2,nas,  12-82,  i2as,  xx  )  ]/DEN2 

(  Qa  ,  1080,  12-ao  ,  1280  ,io) 
aa  ,  1080  ,.  12-82  1  128s  ,10) 

aa, loan, 12-82, 1280,10)] /DEN2 
as , 1080, 1 1  — as  1 x 1  as, 10 ) 

82,  10 Ss ,  x 1 —82,  1 x  8s  , xo ) 
82,1080,  11-82,  xx8jh,  10)  ]/DEN2 

a&  ,  i*a-r ,  iB-a* ,  isav,  1*  ) 

83 ,  X4&"* ,  ib-83,  ioa^ ,  1* ) 

as,  x*a&,  is-as,  iBa«,  1*)  ]/DEN3 
(a*,  lsa?,  io-a«, ,  10 a--,  13  ) 

83,   ISftf,   XS-83,    lflilT   .    13  ) 

83,1384,  lo-as,  isae>,  13)  ]/DEN3 
a* ,  1 3 a^ ,  1  -* -a* ,  1  + a^ ,  ia) 

83  ,    iSftV  ,    14—83  ,    14&7   ,13) 

83,    1306,14-83,    148a,    13)    ]/DEN3 


42 


where  DEN1,  DEN2,  and  DEN3  are  given  with  Equations  2.73. 

The  joint  angles  for  the  supporting  legs  are  computed 
using  information  from  Chapter  1  and  Chapter  2.   The  end 
position  of  each  of  the  supporting  legs  in  body  coordinates 
is, 


2.86 


Em  o  i  j 

liv-o ,  J 

= 

EkO  i  J 

1 

Euler 

rotation 

and 

translation 


Ehw  i  J 

1 


The  joint  angles  are  computed  using  Equations  1.12,  1.20, 
and  1.25. 

This  concludes  the  development  of  the  dynamic 
equations  for  the  body  and  supporting  legs  of  the  walking 
robot. 
2-2.   DYNAMICS  OF  NON-SUPPORTING  LEGS 

As  stated  at  the  beginning  of  this  chapter,  the 
non-supporting  legs  are  treated  as  if  they  were  on  a 
inertial  reference  frame.   This  allows  the  non-supporting 
legs  to  be  treated  as  independent,  three  degree  of  freedom, 
robotic  arms. 

Lagrange's  equations  for  leg  j  are, 

2.87     d_  (3L )    -   9L =  Tt  .  _,  .'  i=2,  3.  4 


d_ 
dt 


39i  ,  j 

In  order  to  write  the  kinetic  and  potential  energies 
of  the  non-supporting  legs,  the  following  definitions  are 
made, 

1-       r,  ,  j  ,  H  ,  ti,jiyi  and  r±  ,  j  ,  •  are  the  x,  y,  s 
components  of  the  center  of  mass  of  link  i, 
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leg  j,  in  the  Xo,  vWl  z0  coordinate  system. 

2-  lnu , j  is  the  distance  from  the  origin  of  the 
i-1  coordinate  system  to  the  center  of  mass 
of  link  i  on  leg  j. 

3-  utiii«i  u>,j,vj  and  w±,^,m    are  the  angular 
velocities  about  the  X± ,  Y» ,  and  Z±  axes  of 
link  i+1  on  leg  j. 

4-  Ii,j,»,  Ii,j)V,,  and  Ii,j,«  are  the  moments  of 
inertia  about  the  principle  axes  of  link  i  on 
leg  j. 

5-  Ii,j  is  the  mass  of  link  i  on  leg  j. 
Calculating  the  values  for  r  gives, 

2.88   r3jJ..«  =  c(ei,j+e=!,j)iito,j+c(ei,  j  )U,j 

r=,J)V  =  S(ei  ,  j+6=,  j)lm=,  j+SCQx,  i  )li,  j 

ri,j,.  =  0 

r3;J,»  =  C(ei,j+6=,.i  )C(e;3,.i)lm3,  j 

r3,j,v  =  S(6i  ,  j+e3,  j.)C(93,  j  Jim*,  j 

+s<e1(J+e=,  j  )iz,  J+s<e1,  j)i.,  j 

r3,  j,_  =  -S(63,  j  )lm=.,  j 

r»,JlM  =  C<ex,  j+e=,  j  )[C{63,  j+e»,  j  )lnw,  j 

+c(83,i)b,J+i2,J]+ciei,J)i.,J 

r»,J)V  =  Sfex  ,  _,+e=,  j  )  [C(6a,  j+6«,  j  )lnw,  j 

+C(e»,j)ia,j+L«,j]+s<ei,j)ii,j 

r-»,  J,«   =   -S(0--5,  j+«»,  J  )  lift.,  J-S(9r5,  j  )  13,  J 

Taking  the  time  derivatives  of  the  r's  yields  (note  9i ,  j  - 
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const. ) 

2.89  r3)J,„    =   -e=,  jSCOx,  j+e3,  j  )lnhs,  j 
r^.j.y   =   9=,  jC(9i  ,  j+e3,  j  )  lm=,  j 
r2,j,.    =   0 

r^.j,,  =  -e=,  jSOx,  j+e=,  j  )[C(e3,  j  )iite,  j+iz,  j] 

-9=,  jC(6x,  j+6a,  j)S(93,j)llto,  j 

r3,j,y   =   9=,jC(9i,  j+e=,  j)[C(e^,  j  )lm=.,  j+l=,  j] 
r3jJ,.    =   -9*,  jC{93,  j  )lite,  j 

r-,j,~  =  -e=, jS(ex, j+e=, j >[C(e3, j+e»,  j)im*, j 

+C<eap,J)l,,J1  +  la,  j] 

-93,jC<9i,j+9i,j  )[S(93,..+9.,  j  )lm»,  j 

+s<e3,J)i3,  j] 

-9*,  jC(9x(J+e=,  j  )S(93,  ^+9»,  j)lm»,  j 
r»,Jjv   =  9z, jC(9., j+ea,j)[C(93,j+e», j )lm»,j 
+C(93,J)1=,_,  +  1=,  j] 
-93,  JS(9x,J+9z,  J)[S(9.3,J+e»,J)lm«,J 

-9,,  jS(9i,  j+9=,  j)S<9=,  j+8,,  j  )lm»,  j 
r~,*,m    =   -9a,  j  [C(9.3,j+9.,  j  )lm»,  j+C(8»,.i  )L=,  i] 
— 9-» ,  j  C  ( 93 ,  j  +9^ ,  j  )  lnw ,  j 
Calculating   the  u>'s   gives 

2. 90  ui,j,»  =  0 
ui,j,»  =  0 
u>,i,.    =   9= ,  j 

w=, J(„    =    -9=, jS(93, j ) 
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)  -i  1  V      -      ~ 9=  ,iC(  §3  ,   J    ) 
=      &>,J 

=    -9a,  jSOs,  j+9»,  j  ) 
=   -9*,  jC[63,j+e«,j  ) 
=   9-3 ,  j  +8* ,  j 
The   kinetic   energy   of    leg    j    is, 
4 

2.91  5  s  1/2  I  {B.,jC(r4,j,«!"+(H,j,y)»+<r,,j„)»] 

i=2 

+  [Il,J,K(Wt-l,    Jj«     )Z 
+  il    ,  J  ,   v  (Wi-1   ,  J   ,  v  )  = 

+  I± ,  j  ,  * (Uft-lj J ,  * )3]} 

The  potential  energy  is, 

2.92  P  =  -tte,  jGS(93,  j  )lm3,  j-m«,  jG[S<93,  j+e*,  j  )lra»,  j 

and   the    Lagrangian    is, 
L=K-P 
Taking  the  required  derivatives   of  L   gives, 

9^,  j  {m= ,  j  lra= ,  j3 
+ite,  j  [C(9*,  j  )  Into,  j  +lm=  ,  j  ]  = 
+m»,  ,,[0(9=,  j +9^,  j)lm»,  j 
+0(93,^)13,^+1=^]  = 

+  I=,   J..+I3,   J,„S=(93,  j) 
+  I3,J,V.C=(93,  4) 

+  I»,j,«iS=(93,  j+9»,  j  ) 

+  I»,  J;yC=(93,  j+9«,  j  )} 

+28=,  j 93,  j  {-S(9.3,  j  )  lite  ,  jots ,  j 


2.93  d_ 

dt 
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2.94 


2.95 


d_     f8L 
dt     \  90= 


") 


dt 


d 


*(C(93,_,)lm=, _,+].=  ,„) 

-aw,  j  [0(9=.,  j+e«,  ^  )S{e3,  j+e*,^ ) 

*lnu,  j=+C(e.i,  j  >S(S»,j)l»,4* 
+S(20=,  j+e^.jjl^.jlm.,  j 

+s(e=»,  j)i=,  j13,  j] 

+C(63,  J)S(8=,J)(l3,J,M-l3,  j,„) 
+C(93,j+9.,  jJS^,  ,.+9.,.,) 
*(I»,  j,»-I-,j,v)} 

+2e2,  je.,  j  {-nu,jS(e3,j+8»,  j) 

*lnw,J[C(93,j+9»,  j)lm»,  j 
+C{8»,j)1»JJi+1»,Ji] 

+C(9,l4te«,J)S(9s,J+94,j) 

=  9s,  j  [nfcs,  j  Inks ,  _»  = 

+nu,  j(lnu,  j  =+2C(9.  ,  j  )1<»,j1ib«,j 


+  1= 


+  Ls,  J,.+I«,  4,«] 


+9,,  j  [m,,  j  ( lm-»,  j  = 

+C<9,,  j)l3,  jlin.,j  )+I«,4,«] 

-293,  j9»,  jS(9»,  ^  )  Is,  j  lnw,  jm», . 

~9.»,  j  =  S(9»  ,  j  )  I3,  j  lm»,  jm»,  j 

9at,  j  [  Ira. ,  j=m. ,  j 

+C  ( 9. ,  j  )  I3 ,  j  1  m« ,  j  m. ,  j  + 1. ,  j  ,  «  ] 

+9.,  j  [  lnu,  j=ra.  ,  j  +  I»,  j  ,  a  ] 

-93,j8.,jS(9»,j)l3,jlm.,jm.,j 
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2.96  9L_   =  0 

2.97  9L_   =  ©»,j»{-ito,j[C(9»,j)8(©«,j  )lm» ,4= 

863,4 

+8<ea,J)l»,JllBB,j] 

-nw,  j  [C(9=,,  j  +9.,  4)S(8=,4+9»,  j  )lm.,  j2 
+3(203,^+0.,  j  )1»,  j  law,  4+3(93,  4+9.,  j) 
*1^, j lm», 4+0(93,4)3(3, j )13)J= 

+S(93,4)l=,jl3,4] 
+C(93,4)S(3,4)(l3,4,--l3,4,v) 

+C(93,4+9.,j)S(93, j+9.,j) 

*(  I-,4,»-I,,4,v)} 

+m=,  jGC(93,4)lm3,4 

+nw,  jG[C(93,  j+9.,4  )  lnw,  4 +C(9.-3,  4)13,4] 

2.98  9_L_    =  9=, j  =  S(9.3,  j+9., 4){-nw,j[C(93,4+9»,j) 
99.,  4 

*lm.,  4  =  +C(93,4)l3,4lm.,4+l=.,4lm.,4] 
+C(93,4+9.,j)(I.,4,»-I.,4,v)} 

+63,  j3ta.,4S(9.,4  )l3,4lm.,4 

+93,  4©.  ,4m.,4S(9.,4)l3,4lm.,4 

+nu,  jGC(93,j+6.,  j  )lm.,  4 
Lagrange's  equations  for  the  non-supporting  legs  are 
written  as, 

2.99  T=,j  =  e\=,j{m=,jlm=,j  = 

+  <Il3,4[C(e3,  4)lm3,4+l=,  4]3 

+m.,4[C(93,  j+9.,  j)lm.,4 
+C(9=, j)l,,4+l=,j]= 

+I=,4,.+l3, J,KS=(e3,4 )+!=, J,VC=(93, j) 
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2.  100 


+i»,  j,vc=(e=,  j+e»,  j)> 

+26a,  jfe,  j{-Dte,  jS<93,  j)lKte,  j 

*[C(8», jjlm,, j+lz, j] 

-bu,  j[C(es,  j+e»,  j)S<9s,  j+9»,  j)lm»,  j= 

+0(93^)3(93, j)13,j  = 

+S(29=J  j+9»,j)1»,  j  law,  j 

+S(9„,  j +9,,  j)l=,  jlnw,  j+S(93,  ..)1=,  j1s,  j] 

+C(93, _. ) S(e3, j ) (I„, j.m-Is, *,*) 

+0(9=,  ..+9..,  j)S(93,  j+9«..  j) 

+29a,  j9.,  j{-m«,  jS(93,  j+9»,  j)lm»,  j 
*CC(93,  j  +e»,  j)lm»,  j+C(93,  j  )13,  «+l»,  j] 
+C(93,  j+9-,  ^  )S(9s,  j+9»,  j  ) 

9s,  j{ib,  j  lnta,  j  =  +m»,  j[lm.,j3 
+2C(9-»,j)l3,jlm,,_, 

+  I3,  j=]+l3,  i,n+U,  -.,.} 

+9.,,  jfnv,,  jlm.,  j[litw,j+C(9»,  j)l3,  j] 

+  U,  J,.}+9=,  43{lte,  jS(93,  J    )lt03,  J 

*[C(93,  j)la»,  _t  +  l=,  j] 

+nu,j[C(93,  j+9»,  j)S(93,  j+9*,j)lm»,  j  = 

+S(293,J+9..,J  )l3,  jlm.,  j+S(93,  j+9»,  _■  ) 

*ia,jim»,^+C(e<»,j)S(e<»,j)i»,ja 

+S(8,,J)l=:,Jl,,J]-C(9,..J)S(9,,J) 
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*S(e3,  J+e„,  t){U,4,m-lm.,  J,v)} 

+293 ,  j  6« ,  j  [  -m* , ..  S  ( e» ,  j  )  I3 ,  j  lm. ,  j  ] 
-9»,  j=[m«,jS(e«,j)l3,j  lm»,  j  ] 
-nts,  jGC(93,  j  )lni3,j 

-m«,jG[C(93,j+e#,i  )lm»,  -.+C(9=,  j  )  1=  ,  j  ] 
2.101        T»,j    =  63j  j{m.,  jlm.,  j[lm.,j+C(e»,  j  )l3,j] 

+L»,  j  , « }+e»,  j  [m»,  j  lm»,  j3+i»,  j  ,  m  ] 

+9=,j  =  S(93,.,+9.»,  j  ){m.,  jlm.,  j 

*[C(93,  j  +9.,  j  )  lnu,  j+C(6=i,  j)  U,  j+1«,j] 

-C(9.,jt94,J)(I«,1,»-I«lJ,v)} 

+93,j2[-ih.,jS(94>j  )l3,  jIhu,  j] 
+283,  j9«,j  [-m-»,  jS(9»,  j  )  Is,  jlnu.,  j  ] 
-nw,  jGC(93,  j+9»,  i)lm.,  j 
Writing   equations    2.99    thru   2.101    in   matrix   form 


gives, 


9=JJ 

T»,j 

Qi 

Si 

M 

03  ,   J 

= 

T3,J 

- 

Q= 

- 

Sa 

9** ,  j 

-  T»  ■• J. 

_Q*_ 

G3 

- 

2.  102 


Where  the  elements  of  £J,  Q,  and  G  are, 

2.103        mi  ,  i    =  m=,  jlm=,  j=+m3,  j  [C(93,  j  )  lute,  j+l=,  j  ]  = 
+m»  ,j[C(93,j+9-.,j)lm«,j 

+C(93, J )l3, J+l^.j]3 

+I=,J,,+S=(93, j)l3, J,m+C=(93, J)l3, _,,v 

+S=(83, _.+8», j) U,j,» 

+C*(9»,j+6*,j)I«,j,v 

mx ,  =  =0 
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BU,3  =0 
Bi,i  =0 
m=,=   =  ma,  j  Into,  j  =  +m»,  j  [  lnu»,  j  = 

+2C(6.,^)l3,Jim.,  j+l»,  j'l+Ia.j  ,.  +  l»,  j,, 
S!=,3   =  m>»,  jlnw,  j  [lm«,  j+C(9»,  j  Jla,  j  ]+I»,  j,. 

ttto,  1=0 

Sfcj,=   =  m=:,.3 

m--3 ,  3    =  ttm  ,  j  lnw ,  j=  +  L*,  j  ,  ■ 
2.104        Gr    =    26=^93,  J{-nte,JS(e3,J  )lite,  j 

*[C(93,  jJiBte,  j+l=,  j]-m»,  j[C(93,  j+e»,  _.) 
*S(93, J+e», j ) lnw, j3+C<9» , j )S(93, j ) U , j" 
+S(2e3,  j+9.,  j)l3,  jlnw,  j+S(93,  j+9-.,  j) 
*1=,  ^lm*,  _.+S(93,  j)1z,  jl3,  j]+C(93,  j) 
*S(93, ^)(I3, j,--I3, j,v)+C<9»,*+e*,j) 
*S(93,J+9.,J)(I»,  j,«-I»,  Jiv)} 
+29=,  ^9*..  j{-tru,  jS(93,  j+9.,j)lm»,  j 

*[C{ea,j+e»,Ji)iau,j+c(ft«,4}i3,j+x*,4  ] 

+C<93, j+9«, j )S(93, j+9., j) 

*  (  **  tilR'Uiijy}} 

G=    =    Z93,j8»,  j[-m»,jS(e«,  j  >l3,  jlnv.,  j] 
_9» ,  j  =  [  nw ,  j  S  ( 9» ,  j  )l»,jlffl.,j] 

G3    =    293, j9», j [-m», jS(9», j )l3, jlm», j ] 
2.  105         Qi    =    0 

8a    =   9=,  j  =  {iB3,  jSO,,  j)  Into,  j[C(9b,  j)lttto,  j  +  l=,  4] 
+nu,  j  [C{©»,Ji+ft.,J,  ) S(9w,j+e»,  j  )lm»,j» 
+S(  29= ,  j  +9* ,  j  )  I3 ,  j  In* ,  j 
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-c(9<ai,J)S{eai,j)(i»,J,»-i»,j)V) 

-C(ew ,  a 4«» ,  j  ) S(6« ,  4  +6»,  j  )  (  X* ,  i  , . -I»,  j  ,  v ) } 

-nti,  jGC(6s,j) int.,  j-io», ..Gccte^, j+e»,  j  ) 

*lm», j+C(e=, j  )1=, j] 
Q=   =   e=,  j=S(83,j+e», j){m», jlm«,j 

*[C(63,J+0-,  j  )lnw,  j  +C(6«,4>l»,j  +  la,j] 

-c(8=>,j+e,,J)(i,,1,K-i.,,,y)} 

+6=,  J2[-BV»,  jS(9.,  j)1=,  jlBU,  j  ] 

-nu,  jGC(83,  j+e»,  j)lm.,  j 
Solving  Equation   2.102   for  the  6 » , j    terms   gives, 

"I.;. 

9. 
where  the  elements  of  M_1  are 
2. 107    nra»,»  =  l/mx,x 


T«,j 

Qx 

Gi 

To,  j 

- 

w= 

- 

G= 

T»,j 

©3 

G3 

m-1! 

a 

= 

0 

m-lx 

■■3 

= 

0 

nr1* 

1 

= 

0 

st1* 

3 

= 

m3,3./DET 

<a-%= 

01 

= 

-m=,3/DET 

E_13 

1 

= 

0 

BT's 

3 

= 

m~  *  = , 3 

!B  '3,3  =  m=,  =  /DET 


where, 


5  2 


DET   -   S3,3gzf2-I!te,33 

The  state  variables  are  defined  as, 
2.  108    y,  =  e3,  ^        y»  =  0=,  j 

y=  =  63 ,  j       ya  =  93 ,  j 

y=  =  9» ,  j       ya  =  6» ,  j 
The  state  equations  in  final  form  are, 
2  .  109    yx  =  yA 

y=  =  y= 
y=  =  y*. 
y»  =  (  T= ,  j  -Qx  -Gx  )  /mx  ,  x 

y=  =  [Bs,3<T3,j-Qa-G=)-ite,3(T*,->-Q3-G3)]/DET 
y*  =  ClBa,  =  (T»,  j-Q3-G3)-nn,3(T3,  j-Qa-G=)]/DET 
This  concludes  the  development  of  the  dynamics  for  the 
non-supporting  legs  of  the  walking  robot. 
2-3.     PHYSICAL  PROPERTIES 

In  this  section  the  physical  properties  of  the  walking 
robot  are  developed.   This  walking  robot  is  intended  to 
simulate  a  small,  lightweight,  remotely  controlled  system 
which  has  some  type  of  terrain  preview  information 
available. 

The  body  is  a  cylinder  18.0  inches  in  diameter,  6.0 
inches  thick,  and  weighs  501bs.   Using  G  =  32.2  ft/a*,  the 
following  body  constants  are  calcuated, 
2. 110    Mb  =  1.55  slug 

I-o  =  0.  250  slug  fta 
lyo  =  0. 250  slug  ft= 
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I.o  =  0.436  slug  ft2 
Link  2,j  measures  2.5  inches  by  2.5  inches  by  3 . 0 
inches  and  has  a  density  of  0.05  lb/in3.   This  density  is 
intended  to  represent  a  strong  aluminum  structure  with  no 
drive  motors  located  on  the  link.   The  constants  for  link 
2,  j  are  calculated  as, 

2. 111  m=;J  =  0.0291  slugs 
I=,j,-  =  0.000211  slug  ft3 
I=,Jjv  =  0.000257  slug  ft= 
I=,j,.  =  0.000257  slug  ft" 
lm*,j  =0.  125  ft 

la, j  =  0.25  ft 
Link  3,j  measures  2.0  inches  by  2.0  inches  by  18.0 
inches  and  has  a  density  of  0.100  lb/in3.   This  density 
again  represents  a  strong  aluminum  structure  but  with  the 
addition  of  drive  motors  for  joints  3  and  4.   The  constants 
for  link  3, j  are  calculated  as, 

2. 112  nb, j  =  0.224  slugs 
U,j,»  =  0.00104  slug  ft= 
Ia»,j,v  =  0.0425  slug  ft= 
I»,j,«  =  0.0425  slug  ft3 
lnt»,  j  =  0.75  ft 

1», j  =  1.5  ft 
Link  4, j  measures  1.5  inches  by  1.5  inches  by  24 
inches  and  has  a  density  of  0.05  lb/in3.   This  density  was 
chosen  for  the  same  reason  as  that  of  link  2,j.  The 
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constants  of  link  4, j  are  calculated  as, 
2. 113    m.,j  =  0.0839  slugs 

I»,j,m  =  0.000218  slugs  ft3 

I*,4,v  =  0.0281  slug  ft3 

U,J;.  =  0.0281  slug  ft= 

lm.,j  =  1.0  ft 

l»,j  =  2.0  ft 
This  ends  the  development  of  the  dynamic  equations  and 
physical  constants  for  the  walking  robot.   The  material 
developed  in  this  chapter  will  be  used  in  the  simulation 
program  and  in  Chapter  4  in  estimating  joint  inertias  for 
the  control  system. 
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III.  RECURSIVE  SPLINE  PATH  CONTROL 


In  prior  chapters  the  kinematic  and  dynamic  equations 
for  a  six  legged  walking  robot  were  developed.   In  the  next 
two  chapters  control  concepts  will  be  developed.   In  order 
to  control  the  walking  robot  it  is  necessary  to  define  a 
desired  path  along  which  it  is  to  walk.   The  path  must  be 
defined  in  the  terms  of  all  six  coordinates  (P„  ,  Pv ,  P.,  <t>, 
9,  + )  which  define  the  position  and  orientation  of  the 
body.   A  parametric  representation  where  the  six  coordinates 
are  specified  as  functions  of  a  parameter  t  is  convenient 
and  natural  if  the  parameter  is  related  to  time.   A 
functional  form  which  satisfies  the  needs  of  this  work  is 
the  uniform  natural  B-spline  function.   B-spline  functions 
have  the  following  properties  which  make  them  particularly 
appealing  for  the  present  application: 

1.  A  B-spline  function  is  invariant  under  linear 
transformations . 

2.  A  Uniform  B-spline  function  of  degree  m  is  m-2 
different iable. 

3.  A  B-spline  function  has  local  support. 

The  second  property  defines  the  smoothness  of  the  path  and 
is  used  in  a  novel  feedforward  control  concept  which  will  be 
developed  in  the  next  chapter.   The  third  property  is  the 
basis  for  the  development  of  a  recursive  algorithm  which 
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permits  the  path  to  be  defined  in  real  time.   The  recursive 
spline  algorithm  provides  the  ability  to  use  vision  or  other 
sensory  inputs  or  operator  commands  to  specify  the  desired 
path  as  it  is  being  traversed.   Finally,  the  use  of  B-spline 
functions  provides  a  smooth  and  graceful  path  with  a 
relatively  small  number  of  data  points. 

In  addition  to  defining  the  desired  path  of  the  body 
of  the  walking  robot  it  is  necessary  to  define  the  desired 
motion  of  the  non-supporting  legs.  The  recursive  B-spline 
function  is  used  for  this  purpose  also. 

3-1.   B-SPLINE  CURVE  FITTING 

The  degree  of  the  spline  function  determines  the 
number  of  continuous  derivatives  and  the  extent  of  the 
support.   A  fourth  degree  (cubic)  spline  will  be  used  in 
this  work.   The  cubic  B-spline  function  on  the  interval 
tK<_t<.ti«  +  l  is  given  by, 

2 

3.1  Q*<t)  =  E  ai.~iNi.H-!  [(t-t.,-!  )/Dt] 

i=-l 

where  the  following  definitions  are  made  relative  to  Figures 
3.1  and  3.2 

3.2  %«  is  the  value  of  the  data  at  t  =  t* 
q' •.  is  the  slope  of  the  curve  at  t  =  t* 
N*  is  the  B-Spline  <N<t))  basis  function 

centered  over  ti. 
a*  is  the  weighting  factor  that  multiplies  N* 
Q*  is  the  cubic  polynomial  defined  over  the 
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interval  t,,<t<_tk~» 
n+1  is  the  number  of  data  points  (knots) 
Dt  is  the  interval  t»,-t*-i 

N  (t) 


-10  1 

FIGURE   3.1      B-Spline   Basis   Function 
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0.0 


ai<-»3*N*-z 


eik+i  *N*-»-i 


tn— 1         t* 


FIGURE  3.2   B-Spline  Curve  Development 
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The  B-spline  basis  function  is  shown  in  Figure  3.1  and 

is  defined  analytically  by  the  equations, 

3-3  f  <t+2);V6  -2<t<-l 

[-3(t+2)=*+12(t+2) 
N(t) 

-12(t+2)+4]/6         -l<t<.0 

N<-t)  0<t<2 

The  values  of  the  spline  basis  function  and  its  first 

derivative  evaluated  at  the  knots  are 

3-4    _t Nit] dN(t)/dt 

-2  0  0 

-1  1/6  1/2 

0  2/3  0 

1  1/6  -1/2 

2  0  0 

The  method  of  fitting  the  spline  function  through  a 
set  of  data  points  is  now  described.   The  data  points  are 
aligned  with  the  knots  so  that  each  data  point  is  equal  to 
the  weighted  sum  of  the  spline  basis  function  evaluated  at 
the  knots,  thus 

3.5  q0  =  a-i  (  l/6)+ao(2/3)+a1  (  1/6) 
qi  =  ao<l/6)+ai(2/3)+a=<l/6) 

q„  =  ao-x(l/6)+ar.(2/3)+a^.-i(l/6) 
This  results  in  n+1  equations  in  terms  of  the  n+3  weights  a» 
(i  =  -1,  0,  ...,  n+1).   Two  additional  equations  are 
required.   These  may  be  obtained  by  specifying  the  slopes  at 
to  and  tr. ,  thus 

3.6  q'„  =  -l/2a-i+0ao  +  l/2ai 
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1'r,     =  -l/2air.-x +0:9^  +  1/28^*1 

Combining  Equations  3.5  and  3.6  and  writing  them  in  matrix 
form  gives, 


-1/2    0    1/2 
1/6   2/3   1/6 

1/6   2/3   1/6 


■            -. 

a-i 

q'o 

ao 

qo 

ai 

= 

qi 

ao— x 

£.-» 

a,. 

q~ 

a„.  . 

q'" 

• 

3.7 


1/6   2/3   1/6 
-1/2    0    1/2 

Solving  the  equation  corresponding  to  the  first  row  of 

Equation  3.7  for  a_x  yields, 

3.8  a-»  =  -2q'o+ai 

Substituting  Equation  3.8  into  the  equation  corresponding  to 
the  second  row  of  Equation  3.7  and  solving  for  ao  yields, 

3.9  ao  =  (3/2)q'o  +  (l/2)q'o-(l/2)ax 
Subsituting  into  each  successive  row  yields, 


k-1 
3.10     a*.    =  E  Pjqu-j+R^qo+SKq'o+iVai 
J  =  l 


k>: 


where, 


3.  11     Px  =  6 

P=  =  -24 

P..  =  -4PJ-X-PJ-2,    i>3 

3.  12     R3  =  -3/2 

R=>  =  12/2 

Rj  =  -4Rj-x-Rj-a,    j>4 

3. 13     S=  =  -1/2 

S=»  =  4/2 

Sj  =  -4Sj—  x-Sj—  =,    j.>4 
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3.  14    Tst  =  -7/2 

Ta  =  26/2 

Tj  =  -4Tj-i-Tj-=,    j>4 
Writing  a„~i  in  the  form  of  Equation  3.10  gives, 
n 

3.15  8n*l   =  Z     Pjqn»l-J+Bn»iqptSn»iq'o 

j=l 

+T„H.,ai 
Using  the  last  row  of  Equation  3.7,  a^-i  can  also  be 
written  as, 

3.16  a„-n  s  2q'r.+ai-.-i 

where, 

n-2 
an— x  =  Z  Pjqn-i-j+En-iqo+Sn-iq'o 

j=l 

+T„-iai 
Equating  Equations  3.15  and  3.16  and  solving  for  at 
yields, 

n  n-2 

3.17  ax  =  [  E  Pjq„«-i-  E  Piqn-i-j 

j=l  j=l 

+  (Kr,-1-Br,-l  )qo+<Sr,-l-Sn-!  ) 

*q'o-2q'n]/(T„-l-Tr,~l)   ,   n>3 
The  a*'s  can  be  solved  by  computing  the  four  series  in 
Equations  3.11  thru  3.14,  then  solving  for  a,  using  Equation 
3.17.   Next,  a_i  and  a»  are  solved  by  using  Equations  3.8 
and  3.9,  and  a=  thru  a^-i  are  solved  using  the  formula, 

3.18  a„  =  6qi>-l-4afc-i-ak-i 
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3-2.     RECURSIVE  PATH  PROPAGATION 

The  development  of  the  method  for  computing  the  cubic 
spline  in  Section  3-1  is  for  fitting  a  curve  through  a  set 
of  n+1  data  points.   In  general,  all  of  the  data  points  must 
be  known  and  the  slopes  at  the  beginning  and  end  of  the 
curve  must  also  be  known. 

In  certain  applications  for  walking  robots,  such  as 
surveillance  or  exploration,  the  terrain  or  the  path  that 
the  robot  must  take  may  not  be  known  in  advance.   Desired 
path  points  may  be  obtained  as  the  path  is  being  traversed. 
This  makes  it  necessary  to  modify  the  construction  of  the 
spline  function  so  that  a  segment  of  the  curve  can  be 
constructed  using  a  few  data  points.   Subsequent  segments 
can  be  constructed  as  additional  points  become  available. 
To  begin  the  curve,  points  q0  through  qK ,  and  the  slope  q'o 
are  used.   By  assuming  the  slope  q'„  j.s  zero,  a  spline  curve 
can  be  drawn  between  to  and  t» .   Since  the  curve  is  drawn 
only  over  one  interval,  a-i  through  a=-  are  the  only  a's 
that  need  to  be  calculated.   Once  the  curve  has  reached  ti , 
a  new  spline  for  the  interval  t,  through  t*  must  be 
developed.   This  is  done  by  dropping  qQ  and  adding  q«»i 
and  shifting  the  q's  by  one.   The  beginning  slope  for  the 
new  segment  is  the  slope  at  ti  from  the  old  segment  and  the 
ending  slope  is  again  assumed  to  be  aero.   By  calculating 
a-i  through  a=  the  curve  can  be  extended  from  ti  to  tz . 
This  recursive  procedure  can  be  repeated  for  as  many 
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segments  as  required. 

Figures  3.3  through  3.5  show  a  recursive  spline  curve, 
along  with  its  first  and  second  derivatives,  through  a  set 
of  randomly  generated  data  points  spaced  at  even  intervals 
along  the  t  axis.   The  curves  in  Figure  3.3  were  constructed 
using  three  data  points  to  draw  each  segment.   The  recursive 
spline  is  smooth  and  looks  very  much  like  the  spline  drawn 
using  the  entire  set  of  data  points  (Figure  3.6).   By 
observing  the  first  derivative  curve,  abrupt  changes  in  the 
slope  can  be  seen  which  show  up  as  discontinuities  in  the 
second  derivative.   Figure  3.4  is  a  recursive  spline  curve 
drawn  through  the  same  set  of  data  points  as  Figure  3.3  but 
using  four  data  points  to  calculate  each  segment.   The 
spline  and  its  first  derivative  are  similar  to  that  of 
Figure  3.3,  but  the  discontinuities  in  the  second  derivative 
have  almost  disappeared.   Using  five  points  (Figure  3.5), 
there  is  no  apparent  discontinuity  in  the  second 
derivative.   Figure  3.6  is  a  spline,  with  its  first  and 
second  derivatives,  drawn  through  the  complete  set  of  data 
points  using  the  method  of  section  3-1. 

The  spline  curves  that  are  drawn  in  a  recursive 
fashion  using  only  a  few  data  points  to  construct  each 
segment  follow  very  closely  the  spline  that  is  drawn  using 
the  entire  set  of  data  points.   When  four  or  more  data 
points  are  used  the  discontinuities  in  the  second  derivative 
term  are  of  no  appreciable  magnitude. 
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FIGURE  3.3   Recursive  Spline  Function  Using 
Three  Data  Points 


FIGURE  3.4   Recursive  Spline  Function  Using 
Four  Data  Points 
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FIGURE  3.5   Recursive  Spline  Function  Using 
Five  Data  Points 
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FIGURE   3.6      B-Spline    Curve 
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3-3.     APPLICATION 

The  data  used  to  construct  the  path  function  for  the 
walking  robot  are  shown  in  table  3.1.   All  the  data  shown 
are  with  respect  to  the  world  coordinate  system. 

The  data  values  for  P„ ,  py,  p« ,  $,  e,  and  +  are 
converted  directly  into  recursive  spline  functions  and  used 
to  guide  the  body  of  the  walking  robot. 

The  data  values  describing  the  endpoints  of  the  non- 
supporting  legs  are  used  along  with  the  body  position  and 
orientation  data  values  to  create  spline  functions  for  the 
individual  joint  angles.   By  using  the  inverse  kinematics  of 
Chapter  1  and  the  Euler  transformations  of  Chapter  2,  the 
corresponding  joint  angle  values  are  computed.   These  joint 
angle  values  are  used  to  construct  the  recursive  spline 
functions  that  control  the  individual  joints. 

For  use  in  the  simulation  program,  the  recursive 
spline  functions  are  written  in  the  form, 

Q«(t)  =  EO=t3+K=t=+Kxt+Ko 
where, 

K.-3  =  <-a-x+3ao-3a*i+a=)/6 
K=  =  (3a-i-6ao+3ai  )/6 
Ki  =  (Sa-L+Sar  )/e 
Ko  -  (a-i  +4ao+ai  )/6 
t  =  (t-t«)/Dt 
The  recursive  spline  functions  developed  in  this 
chapter  will  be  used  in  Chapter  4  and  in  the  simulation 
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program  to  create  the  paths  that  guide  the  body  and 
non-supporting  legs  of  the  walking  robot. 
TABLE  3 . 1  Input  Data  Values 
P~       Pv       P«        «       6       Time(s) 


0.0 

0.0 

1.0 

0.0 

0.0 

0 

0.25 

0.0 

1.0 

0.0 

0.0 

1 

0.5 

0.0 

1.0 

0.0 

0.0 

2 

0.75 

0.0 

1.0 

0.0 

0.0 

3 

1.0 

0.0 

1.0 

0.0 

0.0 

4 

1.25 

0.0 

1.0 

0.0 

0.0 

5 

1.5 

0.0 

1.0 

0.0 

0.0 

6 

1.75 

0.0 

1.0 

0.0 

0.0 

7 

2.0 

0.0 

1.0 

0.0 

0.0 

8 

2.25 

0.0 

1.0 

0.0 

0.0 

9 

2.5 

0.0 

1.0 

0.0 

0.0 

10 

2.75 

0.0 

1.0 

0.0 

0.0 

11 

3.0 

0.0 

1.0 

0.0 

0.0 

12 

3.25 

0.0 

1.0 

0.0 

0.0 

13 

3.5 

0.0 

1.0 

0.0 

0.0 

14 

3.5 

0.0 

1.0 

0.0 

0.0 

15 

3.5 

0.0 

1.0 

0.0 

0.0 

16 

L.,i     Ev~,i     E.w,  i     Exw,=     Time(s) 


0.0 

3.0 

0.0 

0.0 

1.5 

0 

0.0 

3.5 

0.0 

0.5 

1.5 

1 

0.0 

4.0 

0.0 

0.0 

1.5 

2 

0.0 

4.0 

0.0 

0.0 

2.0 

3 

0.0 

4.0 

0.0 

0.0 

2.5 

4 

0.0 

4.0 

0.0 

0.  0 

3.0 

5 

0.0 

4.0 

0.0 

0.0 

3.5 

6 

0.0 

4.5 

0.0 

0.5 

3.5 

7 

0.0 

5.0 

0.0 

0.75 

3.5 

8 

0.0 

5.5 

0.0 

0.5 

3.5 

9 

0.0 

6.0 

0.0 

0.0 

3.5 

10 

0.0 

6.0 

0.0 

0.0 

4.0 

11 

0.0 

6.0 

0.0 

0.0 

4.5 

12 

0.0 

6.0 

0.0 

0.0 

5.0 

13 

0.0 

6.0 

0.0 

0.0 

5.5 

14 

0.0 

6.0 

0.0 

0.0 

5.5 

15 

0.0 

6.0 

0.0 

0.0 

5.5 

16 

*,-,« 

E«:  w  J  2 

Em  w  ,  3 

EyW  !    3 

Ezm  ,   3 

Timet s) 

2.598 

0.  0 

-1.5 

2 

.  598 

0.0 

0 

2.  598 

0.0 

-1.0 

2 

.598 

0.5 

1 

2.598 

0.0 

-0.5 

2 

.598 

0.0 

2 
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2.598 

0.5 

-0.5 

2 

.598 

0.0 

3 

2.  598 

0.75 

1 

-0.5 

2. 

598 

0.0 

4 

2.598 

0.5 

-0.5 

2 

.598 

0.0 

5 

2.598 

0.0 

-0.5 

2 

.598 

0.0 

6 

2.598 

0.0 

0.0 

2 

598 

0.5 

7 

2.598 

0.0 

0.5 

2 

.598 

0.7 

3 

2.598 

0.0 

1.0 

2. 

.598 

0.5 

9 

2.598 

0.0 

1.5 

2. 

598 

0.0 

10 

2.  598 

0.5 

1.5 

2. 

598 

0.0 

11 

2.598 

0.75 

1.5 

2. 

.598 

0.0 

12 

2.598 

0.5 

1.5 

2. 

598 

0.0 

13 

2.  598 

0.0 

1.5 

2. 

.598 

0.0 

14 

2.598 

0.0 

1.5 

2. 

598 

0.0 

15 

2.598 

0.0 

1.5 

2. 

598 

0.0 

16 

i*JM  *>#.■<*■ 

iy  v«  ,  4 

&m  v*  ,  ** 

.Em***  ,  a 

liv  **«  t  a 

Time(s) 

-3.0 

0.0 

0.0 

-1.5 

-2.598 

0 

-3.0 

0.0 

0.0 

-1.0 

-2.598 

1 

-3.0 

0.0 

0.0 

-0.5 

-2.598 

2 

-2.5 

0.0 

0.5 

-0.5 

-2.598 

3 

-2.0 

0.0 

0.75 

-0.5 

-2.598 

4 

-1.5 

0.0 

0.5 

-0.5 

-2.598 

5 

-1.0 

0.0 

0.0 

-0.5 

-2.598 

6 

-1.0 

0.0 

0.0 

0.0 

-2.598 

7 

-1.0 

0.0 

0.0 

0.5 

-2.598 

3 

-1.0 

0.0 

0.0 

1.0 

-2. 598 

9 

-1.0 

0.0 

0.0 

1.5 

-2.598 

10 

-0.5 

0.0 

0.5 

1.  5 

-2. 598 

11 

0.0 

0.0 

0.75 

1.  5 

-2.598 

12 

0.5 

0.0 

0.5 

1.5 

-2.598 

13 

1.0 

0.0 

0.0 

1.5 

-2.598 

14 

1.0 

0.0 

0.0 

1.5 

-2.598 

15 

1.0 

0.0 

0.0 

1.5 

-2.598 

16 

E ,= 

£Lk  *• 

i  ,  &. 

E^_,<. 

E*:  m  >  & 

Time(s 

0.0 

1. 

5 

-2. 

598 

0.0 

0 

0.5 

1. 

5 

-2. 

598 

0.0 

1 

0.0 

1. 

5 

-2. 

598 

0.0 

2 

0.0 

2. 

0 

-2. 

598 

0.5 

3 

0.0 

2. 

5 

-2. 

598 

0.  75 

4 

0.0 

3. 

0 

-2. 

598 

0.5 

5 

0.0 

3. 

B 

-2. 

598 

0.0 

6 

0.5 

3. 

5 

-2. 

598 

0.0 

7 

0.75 

3. 

5 

-2. 

598 

0.0 

8 

0.5 

3. 

5 

-2. 

598 

0.0 

9 

0.0 

3. 

5 

-2. 

598 

0.0 

10 

0.0 

4. 

0 

~2. 

598 

0.  5 

11 

0.0 

4. 

5 

-2. 

598 

0.75 

12 

0.0 

5. 

0 

-2. 

598 

0.5 

13 
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0.0 

5.5 

-2.598 

0.0 

14 

0.0 

5.5 

-2.598 

0.0 

15 

0.0 

5.5 

-2.598 

0.0 

16' 
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IV.  CONTROL  SYSTEMS 


The  control  systems  used,  in  the  simulation  of  the 
walking  robot  are  developed  in  this  chapter.   The  use  of 
polynomial  spline  functions  for  the  input  signals  allow  the 
use  of  a  novel  feedforward  concept  as  well  as  feedback 
compensation,  thus  providing  the  systems  a  degree  of 
anticipation  of  the  control  input. 

4-1.     CONTROL  OF  THE  BODY 

In  order  to  maintain  coordinated  control  over  the 
supporting  legs  of  the  walking  robot,  the  position  and 
orientation  of  the  body  are  used  in  the  feedback  control 
system.   Once  the  required  constraint  forces  are  calculated 
by  the  control  system,  the  Lagrangian  multipliers  of  Chapter 
2  are  used  to  calculate  the  required  joint  torques  of  the 
supporting  legs  to  correctly  drive  the  body  of  the  walking 
robot. 

A  simplified  linear  model  of  the  body  is  used  for 
developing  the  body  control  system.   Neglecting  all 
nonlinear  and  coupling  effects  yields  a  model  of  the  form 
Km/Js2,  where  Km  is  the  generalized  gain  constant  and  J  is 
either  the  mass  of  the  body  (for  P„ ,  P*,,  and  P«  )  or  the 
moment  of  inertia  about  a  given  axis  (for  *,  8,  and  +). 

The  feedforward,  feedback  control  system  that  is  used 
is  shown  in  Figure  4.1.  Kd  is  a  derivative  feedback  gain,  Gc 
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FIGURE  4. 1   Feedforward  Control  System 
is  a  series  compensator,  and  Gff  is  the  feedforward 
controller.   By  simplifying  the  inner  loop,  the  control 
system  can  be  shown  in  the  form  of  Figure  4.2.  The  form  of  G 


4.  1 


Km 


Js=+KmKds 


FIGURE  4.2   Simplified  Feedforward  Control 
System 
Letting  Gff  =  1/G  and  solving  for  C  gives, 
4.2      C  =  R(l+GGc)/(l+GGc)  =  R 

Equation  4.2  indicates  that  the  feedforward  controller 
produces  a  system  where  the  output  follows  the  input 
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regardless  of  the  form  of  Gc.   Since  G  is  a  simplification 
of  the  walking  robot's  dynamics,  Gff  will  not  be  the  exact 
compensator  required  for  the  output  to  exactly  follow  the 
input.   For  this  reason,  Gc  is  used  to  gain  better  control 
of  the  system  when  the  control  model  does  not  exactly  fit 
the  dynamic  model. 

Since  there  is  derivative  feedback  in  G,  Gc  is  chosen 
to  be  a  proportional-integral  compensator  of  the  form, 

4.3      Gc  =  Kp+Ki 


In  order  to  calculate  the  gains  for  the  compensator, 
the  feedforward  loop  is  removed  yielding  the  control  system 
in  Figure  4.3.  The  gains  are  picked  to  give  good  response 
characteristics  when  G  and  Gff  do  not  match  the  dynamic 
model. 


R- 


FIGURE  4.3   Feedback  Control  System 
Solving  for  the  characteristic  equation  of  the  system 
in  Figure  4.3  gives, 


4.4 


s3+KmKds^+KmKES+Kmiii  =  0 
J      J     J 


Since  the  term  Km/J  appears  with  each  of  the  gains  Kd,  Kp, 
and  Ki,  the  value  of  Km/J  is  picked  so  that  the  remaining 
gains  will  have  moderate  values. 
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The  roots  of  the  characteristic  equation  are  picked  to 
be  -1+il,  -5,  resulting  in  a  natural  frequency  of  1.414  r/s 
and  a  damping  ratio  of  0.7071  for  the  dominant  roots.   The 
Km/J  constant  is  picked  to  be  0. 4,  resulting  in  the 
following  gain  values, 

4.  5      Kd  =  17.5 

4.6  Kp  =  30.0 

4.7  Ki  =  25.0 

For  the  body  position  variables,  P„ ,  pv,  and  P..  the 
value  for  J  is  the  mass  of  the  body,  1.55  slugs,  giving  Km  a 
value  of  0. 62  lb/ft. 

The  inertia  that  the  body  orientation  angle  *  sees  is 
I«,oC(e)+Iv.,c.S(6).   Thus  for  <t>,  J  =  0.  436*C(6)  +0.  250*S(6) 
slug  ft3  and  Km  =  0. 4*J  ft  lb  /  radian. 

The  inertia  that  the  body  orientation  angle  9  sees  is 
I>,e  giving  J  =  0.250  slug  ft3  and  Km  =  0.  1  ft  lb  /  radian. 

For  the  body  orientation  angle  +,  the  inertia  is  I«,o 
so  J  =  0.436  slug  ft=  and  Km  =  0.174  ft  lb  /  radian. 

In  order  to  transform  the  required  body  forces  into 
joint  torques  for  the  supporting  legs  the  Lagrangian 
multipliers  of  Chapter  2  are  used.   T»  thru  T*.  are  defined 
as  the  constraint  forces  on  the  body  in  the  P„ ,  PVJ  P» .  <)>, 
6,  and  +  directions  respectively  and  are  given  as, 


4.8      Ti  =  £  i.a,,i  i  =  l,6 

m=l 
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By  letting  \x    =   X=+X3,  X=  -   X«+X*,  X-,  =  X^+Xs,  and  the 
T*  equal  the  required  body  forces,  Equation  4.8  is  reduced 
to  a  set  of  6  linear  equations  with  6  unknowns  (X=,  Xst,  X», 
x*,  }..-?,    Sm).      These  X's  are  obtained  using  a  Gaussian 
elimination  method. 

By  defining  Ti , j  as  the  torque  forcing  9i , j ,  Equations 
2.52  through  2.60  can  be  rewritten  as, 


4.9      T= ,  p  s  —  £  Xm  am ,  T 

m=l 
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These  supporting  leg  torques  are  used  in  the  dynamic 
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equations  to  calculate  the  motion  of  the  body. 

4-2.     CONTROL  OF  THE  NON-SUPPORTING  LEGS 

The  control  system  used  for  each  joint  of  the 
non-supporting  legs  is  of  the  same  form  as  the  system  used 
to  control  the  body  as  shown  in  Figure  4.1.   The  inputs  to 
these  systems  are  spline  functions  describing  the  individual 
joint  trajectories  and  the  feedback  is  from  the  individual 
joints. 

After  experimenting  with  several  values,  the  roots  of 
the  characteristic  equation  (Equation  4.4)  are  picked  to  be 
-2±i2,  -8,  resulting  in  a  natural  frequency  of  the  dominant 
roots  of  2.828  r/s  and  a  damping  ratio  of  0.7071.  The  Km/J 
constant  is  left  at  0.4  resulting  in  the  following  values 
for  Kd,  Kp,  and  Ki, 

4.  10     Kd  =  30.0 
Kp  =  100. 0 
Ki  =  160. 0 

The  values  of  the  moments  of  inertia  about  the  joint 
axes  are  computed  at  a  median  leg  position.  The  values  of 
the  joint  angles  at  this  position  are, 

4.  11     9=>J  =0.0  rad 

8=, j  =  -0.601  rad 
9*,.,  =  1. 78  rad 

By  defining  Ji  ,  j  as  the  moment  of  inertia  about  ©«.  ,  j  , 
the  equations  for  moments  of  inertia  are, 

4.12    J=jJ  =  [I=,  j  ,«+m=,  jlm=,  j  =  ]+[I=5.  j  ,  v  C(&s,j) 
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■m»,  j  (la,  j+Iiib,  iCte^.j))2] 

+m»,  j(  1=,  j  +  l3,  j  0(03,  j  ) 
+  lm»,  jC(9=,  j+8*,  j)  )  =  ] 
■J3,j    =    [  I3 ,  1 ,  m  +nt3  ,  j  lnts ,  jz ] 

+  [I», j, *+m», j ( la, j3+lm»,  j3 
-2I3,  jIhu,  jC(  -fl»,j))] 
«J*  j  j  =  I*  ,  j  , «  +nt»  i  j  lm.  ,  j  2 
Performing  these  operations  gives, 
4.  13     Ja.j  =  0.  511  slug  ft= 
J3,j  =  0. 417  slug  ft= 
J»,  j  =  0.  112  slug  ft3 
The  gain  constant  Km  can  be  calculated  for  each 
joint.   These  calculations  give, 

4.14    Kite.j  =  2.04  ft  lb  /  radian 
Km3,j  =  0.167  ft  lb  /  radian 
Km. ,  _,  =  0 . 0448  ft  lb  /  radian 
This  completes  the  development  of  the  control  systems 
for  the  walking  robot.   The  control  systems  developed  in 
this  chapter  are  used  in  the  simulation  program.   The 
performance  of  that  simulation  are  given  in  the  next 
chapter. 
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V.  RESULTS  AND  RECOMMENDATIONS 


In  Chapter  1  the  kinematic  and  inverse  kinematic 
equations  of  a  six  legged  walking  robot  were  developed.   In 
Chapter  2  the  dynamic  equations  describing  the  six  degree  of 
freedom  motion  of  the  body  with  three  supporting  legs  was 
developed.  Constraint  equations  were  developed  based  on  the 
end  points  of  the  supporting  legs  remaining  at  a  fixed 
location  in  world  coordinates.   Dynamic  equations  were  also 
developed  for  the  three  non-supporting  legs.   In  Chapter  3  a 
recursive  spline  technique  was  developed  for  describing  the 
desired  paths  of  the  body  position  and  orientation  and  the 
joints  of  the  non-supporting  legs.   In  Chapter  4  control 
systems  were  developed  for  the  body  and  the  joints  of  the 
non-supporting  legs  using  derivative  feedback,  proportional 
plus  integral  series  compensation,  and  a  novel  feedforward 
compensation  which  takes  advantage  of  the  differentiability 
of  the  spline  input  functions.   All  of  these  developments 
have  been  integrated  into  a  simulation  of  the  walking 
robot.   The  simulation  is  written  in  Fortran  77  and 
implemented  on  a  Vax  750  computer  system.   A  documented 
listing  of  the  Fortran  source  code  and  input  files  is 
included  in  Appendix  2.   In  this  chapter  the  response  of  the 
simulation  will  be  presented  and  discussed.   The  objectives 
of  this  work  were  to  develop  a  tool  for  the  study  of  the 


77 


application  of  advanced  control  concepts  to  articulated 
mechanisms  and  to  initiate  the  study  of  the  use  of  a 
recursive  spline  path  planning  and  a  novel  feedforward 
control  concept  for  these  mechanisms.   The  simulation  was 
successful  in  meeting  these  objectives.   Recommendations  for 
further  study  are  given. 

5-1.   SIMULATION  RESULTS 

Figures  5.1  through  5.9  show  the  results  of  the 
simulation.   The  body  position  and  orientation  along  with 
the  joint  angles  of  leg  5  are  shown.   Since  all  of  the  legs 
follow  similar  type  paths,  leg  5  was  choosen  as  the 
representative  leg.  In  Figures  5.1  through  5.6,  the  times  at 
which  the  body  weight  is  transferred  from  one  set  of  legs  to 
the  other  are  marked  by  X's  along  the  time  axis. 

Figure  5.1  shows  the  forward  motion  of  the  body,  PM . 
and  the  command  input  versus  time.   As  may  be  observed,  the 
body  response  gets  ahead  of  its  input  by  a  very  small  amount 
at  the  beginning  of  the  simulation  and  maintains  an  even 
distance  until  the  body  stops  at  14  sec. 

The  side  to  side  motion  of  the  body  is  represented  by 
Pv  and  is  shown  in  Figure  5.2.   The  response  remains  very 
close  to  aero,  the  command  input,  with  the  largest  deviation 
being  about  2*10-*  ft. 

The  vertical  position  of  the  body,  P.,  is  shown  in 
Figure  5.3.   The  largest  deviation  from  the  input  is  less 
than  2*10-*  ft.   The  body  does  not  sag  under  the  force  of 
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FIGUEE  5.5  9-Spline  input  and  response 
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FIGURE  5.6  +-Spline   input   and   response 
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FIGURE  5.7  6a,H-Spline  input  and  response 
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FIGURE  5.9  6«,B-Spline  input  and  response 

gravity  at  the  beginning  of  the  simulation  because  there  is 
a  corrective  force  added  by  the  control  system  to  compensate 
for  the  pull  of  gravity  in  the  P«  direction. 

The  body  orientation  angles,  *,  ©,  and  ♦,  are  shown  in 
Figures  5.4  through  5.6.   Again  the  response  follows  the 
input  very  closely  with  the  largest  deviation  being  about 
2.5*10-=  rad. 

The  joint  angles  of  leg  5  are  shown  in  Figures  5.7 
through  5.9.   During  the  periods  marked  SUPPORT  PHASE  the 
joints  are  being  driven  by  the  body  control  system.   During 
the  remainder  of  the  simulation  the  joints  are  being 
controlled  using  the  joint  spline  curves  and  the  non-support 
leg  control  systems  as  described  in  Chapter  4.   The  control 
torques  at  83,3  and  €U,=  have  corrective  terms  added  to  them 
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to  reduce  the  effects  of  gravity  on  the  joints.   These 
correction  terms  help  in  the  smooth  transition  from  the 
support  phase  to  the  non-support  phase. 

It  may  be  observed  from  the  figures  that  the  response 
of  the  body  is  better  than  the  response  of  the  joint 
angles.   This  may  be  due  to  the  fact  that  the  model  used  in 
developing  the  control  system  represents  the  dynamic  model 
of  the  body  very  closely  (this  is  especially  true  at  low 
velocities  of  the  orientation  angles  *,  Q,    and  + ) .   The 
model  for  control  of  the  non-supporting  legs  was  developed 
at  a  median  leg  position.   Changes  in  inertia  at  different 
leg  positions,  torque  coupling  between  joints,  and  switching 
between  supporting  and  non-supporting  phases  produced  some 
deviation  of  the  joint  responses  from  the  inputs. 

5-2      RECOMMENDATIONS  FOR  FURTHER  STUDY 

With  the  research  tool  provided  by  this  study  the 
possibilities  for  further  work  are  extensive.   Further  work 
might  focus  on  altering  the  physical  system  that  was 
modeled,  modifying  or  extending  the  system  model,  evaluating 
the  recursive  spline  path  planning  algorithm,  or  improving 
the  individual  control  strategies  and  systems.   Some 
specific  recommendations  concerning  this  study  are  given  in 
the  following  paragraphs. 

As  the  speed  of  the  walking  robot  is  increased  and  as 
the  control  paths  become  more  complicated,  such  as  walking 
on  inclines  or  over  uneven  terrain,  there  may  be  a  need  for 
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a  more  complete  dynamic  model.   The  beginning  of  the 
development  of  a  more  complete  model  is  given  in  Appendix 
1.  In  this  model,  Lagrange's  equations  are  used  to  write  the 
equations  of  motion  for  the  entire  walking  robot,  with  all 
the  masses  and  inertias  of  all  the  links  being  included  in 
the  model.   The  constraint  equations  are  the  same  as  those 
used  in  the  simplified  model,  that  is,  the  end  points  of  the 
three  supporting  legs  must  remain  at  a  fixed  location  in 
world  coordinates.   Other  sets  of  constraints  could  be 
considered.   Modeling  slippage  of  the  end  point  on  a  smooth 
surface  or  plastic  deformation  of  the  supporting  surface  are 
possibilities.   The  use  of  a  Roll,  Pitch,  Yaw  orientation 
system  on  the  body  rather  than  the  Euler  angles  should  be 
considered.   There  are  two  reasons  for  this  recommendation. 
First,  in  order  to  go  from  a  level  body  position  to  a  small 
rotation  about  the  Y0  axis,  using  Euler  angles,  requires 
step  changes  of  90°  in  the  <t>  and  +  angles.   The  use  of 
spline  functions  to  create  the  paths  for  *  and  +  prohibit 
these  step  changes  in  their  values.  Second,  when  using  the 
Euler  orientation  angles,  the  mass  matrix,  M,  in  Equation 
2.74  becomes  singular  when  the  body  orientation  angle  8  is 
zero.   This  makes  it  necessary  to  use  a  second  set  of 
equations  in  the  simulation  program  to  solve  for  the  body 
orientation  when  8  becomes  very  small. 

The  recursive  spline  path  definition  functions 
developed  in  Chapter  3  are  for  cubic  splines  with  equally 
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spaced  knots.   The  use  of  higher  order  splines  might 
eliminate  the  discontinuity  in  the  second  derivative  when 
using  a  small  number  of  points  to  represent  the  curve. 
Also,  some  consideration  to  using  unequally  spaced  knots 
might  be  given.   The  advantage  of  unequal  knot  spacing  would 
come  in  areas  where  the  knots  need  to  be  kept  close  together 
for  more  accurate  path  representation,  but  where  a  high 
speed  is  also  required.   For  example,  knots  could  be  spaced 
at  one  second  intervals  for  course  paths  and  at  1/2  second 
intervals  for  more  accurate  paths. 

The  control  systems  used  in  the  simulation  program 
were  not  extensively  analyzed.   While  the  body  control 
system  worked  extremely  well  for  the  motions  considered  some 
work  needs  to  be  done  with  the  relationships  between  the 
Lagrangian  multipliers  to  insure  the  torques  at  the  joints 
of  the  supporting  legs  are  well  balanced.   Also,  the  matrix 
formed  from  Equation  4.8  becomes  singular  when  the  body 
orientation  angles  become  zero.   This  makes  it  necessary  to 
add  a  line  in  the  GAUSSX  subroutine  that  does  not  allow  the 
row  divisor  A(i, j)  to  become  zero.   Evaluation  of  the 
performance  of  the  body  control  system  for  other  types  of 
motion  is  suggested. 

The  non-supporting  legs  might  be  a  good  place  to 
implement  a  model  referenced  adaptive  control  strategy. 
While  many  adaptive  control  algorithms  are  too  lengthy  to  be 
used  in  real  time  control,  an  algorithm  to  adaptively  adjust 
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the  gains  in  the  feedforward  controller  might  be  simple 
enough  to  realize  real  time  implementation. 

The  addition  of  interactive  input  to  permit  dynamic 
control  of  the  path  of  the  walking  robot  would  be  a  valuable 
extension  to  the  work.   The  addition  of  interactive  input 
would  require  the  addition  of  graphics  to  display  the  motion 
of  the  body  and  legs.   The  addition  of  a  hierarchical 
control  structure  which  could  receive  sensory  and  command 
inputs  and  produce  path  planning  data  would  simulate  a  more 
autonomous  robot. 

This  thesis  has  presented  the  results  of  a  project  to 
develop  a  tool  for  the  study  of  the  application  of  advanced 
control  concepts  to  articulated  mechanisms.   The  study  of 
two  advanced  control  concepts  has  been  initiated  with 
interesting  results.   Many  suggestions  for  useful  contin- 
uation of  the  work  have  been  presented. 
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APPENDIX  I 
Full  Dynamics 

Appendix  1  is  the  partial  development  of  the  full 
dynamic  model  for  the  walking  robot  described  in  Chapters  1 
and  2.   All  the  definitions  concerning  the  physical  model 
are  the  same  as  those  of  Chapters  1  and  2.   However,  the 
development  in  this  appendix  accounts  for  the  masses  and 
inertias  of  all  the  links  on  all  the  legs  and  of  the  body 
mass  and  inertia  in  one  set  of  Lagrange's  equations. 

As  in  Chapter  2,  the  kinetic  energy  of  the  body,  Kb, 
is, 

Al.l     Kb  =  l/2Mb(P„  =  +Py=+PI  =  )+l/2(l„,0W=M,o  + 

Recalling  that  ut,j,H,  u,,,,v,  and  «,,,«  are  the 
angular  velocities  of  link  i+1  about  the  X» ,  Yi  ,  Z±    axes, 
the  w's  are, 

ai.2   wi,j,„  =  ect^+ex,  j+e=,  j  )+«3(e)S('i'+ei,  j+e=,  j ) 
ui,j,y  =  -esoi'+Qi ,  j+e=,  j  )+4>s(e)C(++6i ,  j+e=,  j ) 

ui,j,,  =  ++*C(8)+6=,j 

+*cs(e)S(++et,j+9a,j)C(8»,j) 

-c(e)S{03jJ)] 

-+ s ( e3 , j ) -e= , j  s ( 6t , j ) 

ui,i,y  =  -9C(*+9i , j+6= , j ) S(8^ , j ) 

-*[s<8)S(  +  +e1,  „+ea,J)s<e3S,  j  i 
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<^=,j,-  =  -es<++6i , j+e=,j  )+is(6)C(++ex,  j+e=,  j  > 
"=,j,«  =  ec(++6i,  j+e=,  jjcte^,  j+e»,  j  ) 

w».4,v  =  -ec(++6i , j+e=, j )S(e=, j+e^, j ) 

+C<9)C(93, J+e», j )]-+C{9»,j+9*,j ) 
-9=, jC(83,j+8.,j ) 

W3,j,»  =  -9S<')'+9i ,  j+e=,  j  )+is(6)C<++ex ,  j+e=,  j  j 

B«ii,«i  Rt.jjyi  and  Ei  ,  j  , .  are  defined  as  the 
coordinates  of  the  center  of  mass  of  link  i,  leg  J  in  the 
world  coordinate  system  (X~,  Y„ ,  Zw ) .   The  values  for  the 
R's  are  given  by  the  following  equation, 

A1.3 


R*.  ,j,m 

Eu  1  ar 

ri  ,  j 

Ri  »  j  ,  v 

= 

rotation 

ri  ,  j 

Ri.  ,j,i 

and 

ri  ,  i 

i 

trans lation 

1 

Where  the  r's  define  the  centers  of  mass  of  the  links  in  the 
Xo,  Yoi  Zu  coordinate  system  and  are  given  in  Equation  2.88. 

Performing  the  multiplication  in  Equation  A1.3  for 
each  of  the  links  of  leg  j  gives, 
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A1.4  Rsb.j.h    =    CO)CO)C(9i  ,  j +9=,  j  jlite,  ., 

+c(*)C(+)C(el;j)lx,J 

-s(*)C(e)S(+)C(ex,j+©a)J)imB,J 

-s<<t>)C(e)S(+)C(8»,J)i1JJ 

-C(*)S<+)S(9l,J+e=,  j  )  late,  j 
-C(*)S('l')S(9x,J)ll,J 

-s<*)G(e)C(+)s{s».M+ea,J)iiB»Jj 

-S(*)C(9)C('(')S(9llJ)ll,J+P„ 
R=,J)V    =    S(4>)C(*)C(9i  ,  j+9=>j  )lm=,  j 

+sct«)C(+)C(ex,i,)ii,J 

+CO)C(9)SO)C(91,J+9=,J)ltte,J 
+CO)C(9)SO)C(91,J)l1,J 
-SO)SO)S(91;J+9=,  jjlite,  j 
-S(*)S(+)S(0i,JJl1,J 
+C(*)C(9)C(+)S(9l,J+9=,J)lite,  j 
+CO)C(9)CO)S(91,J)l1,J+pv. 
Sz,j,>    =    S(9)SO)C(9i  ,  j+9=,  j  )  lite,  j 

+S(9)s<*)C(ex,J)i»,J 

+S{9)C<*)S(9l,J+9=,J)lrte,J 
+S(9)C(*)S(91,J)11,J+P« 
&3,j,»    =    C(*)C(+)C(9i,  j+9=,  j)l=,  j 
+C(«)C(*)C(8,,J)11,J 
-S(4>)C(9)S(+)C(9l,J+9=,J)l=,  j 
-S(<t)C(9)S(*)C(91,J)ll,J 
-CO  )S<+  )S(9x  ,  j +9=,  j  )  1=,  j 

-co)SO)  s<el,J)i1,  J 
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-s(<t>)c<e)C(+)S(eXj4+es,,J)i,,J 
-s(*>c<e)C(+)S(8x,J)il,J 

-s(«)C(e)S(  +  )C(e1)J+e=,J) 

*C(9=,  jjlni,,  j-C(*)S{*)S(9xJ  j+e=,  j  ) 

*c  ( e» ,  j  )  lite ,  j  -s  ( * ;  c  ( e )  c  ( + ) 

*S<8i,j+ea,j)C(8»,j  )liita,  j-S(<P)S(9) 

♦so^.jHiib.j+f. 
r=,..,v  =  s(<f)C(+)C(ex,J+e»,j)ia.,J 
+s<*)C(+)c<e»,J)i,JJ 
+c<*)C(©)S(*)C(et,J+ea,J)i!,,J 

+C(<t>)C(9)S(>IOC(9l,J)l1,J 

-s<*)s<+)S(e1,J+6a)J)i»,J 

+c(*)C<e)c<+)s<e»JJ+ea,J)Xa,J 
+c(*>C(e)C(*)S(91,J)i1,J 

+S(<t>)C(*)C(81,  J+e=,  jJCO^,  jilrte,  j 

+c{«)C(9)s<  +  )C(e1,J+e=,J) 

*C{9=!,  j  )lrte,  j-S<<P)S(  +  )S(9i,  j +9=,  j  ) 

*c(e=»,  j )  lite,  j+c(*)C(9)c<*) 

*S(8»,4+ea,j  )C(8=,  j  )  lite,  j+C(*)S(9) 
*S(83jJ)lite,  j+Pv 
Bb.j,.    =    3(9}S(+)C(9x , j+9=, j ) la, _, 
+S(9)S(+)C(91,J)11,  j 
+S(9)C(+)S(9l;J+e=,  4)1«,  j 
+S(9)C(+)S(9l,J)l1,J 
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+s(e)S(  +  )C<e1 ,  ,+«=,  s  sc<e»,  j  )  lifte, „ 
+s<e)C(+)S(e1,J+e3,J)C(e:=,J)i.-fe,J 

EU..J,-    =    C<<t>)C(  +  )C(9i,j+e=,  j}1»,j 
+C{<0)C(  +  )C(8ljJ)ii)J 

-S(*)C(0)S(+)C(8l..J+93,J)l=,  ^ 
-S(*)C(8)S(+)C(91,J)lllJ 

-c(*)s<+)s<e*,j+0a,,J)ia,,  j 

-C(*)S{+)S(6»,J)l»,J 

-so>)C(e)C(+)S{9»,J+e«,J)ia,,J 

-S(*)C(9)C(*)S(9l,J)lljJ 
+C(*)C(+)C(91,J+8=,  j  )  C(8=,  j)l3,  j 
-S(*)C(9)S(+)C(e1,J+8=,J) 

*C(8ajJi)ia.,j-c(*)S('i')S(ei,j+ea,j5 

*C(93,  j  >1»,  J-S(*)C(9)C(  +  ) 
*S(93,J)l3,J 

+c(0)C(  +  )C(9l,J+e=..J) 

*C(83, j +8», j } lm», j 
-S(*)C(9)S(+)C(91,J+8=,J) 
*C(83,j+8»..  jjlm*,  j 

-c(«)S(  +  )S(8ljJ+e=,J; 
*c(93;J+e»,  j )  iia»,  j 

-S(*)C(9)C(*)S(81,  _,+«=,  j) 
*C(8i  ,  j+6»,  j  )  law,  j 
-S(»)S(8)S(e,,Jt9.,J  )lm»,  j+Pm 


93 


R«,j,„  =  s(*)C(+)C(e»,j+ea,j)i»,j 

+s(*)c<*)C(e1,J)i1,J 
+c{+)c<e)S(+)C(ei,J+ea,J)ia,J 

+C(*)C(e)S(*)C(e1,J)i1,J 
-s(«)S(  +  )S(e1,J+e=,J)i=,  j 
-S{t)S(+)S(e,,4)u,j 
+C{t>)C(e)C(+)S(e*,J+e.,J)i»,J 

+c(«)C(e)C(+)S(ei,J)il>J 

+c(*)C(e)S(+)C(e1,J+e=,J) 

*C(9=,J  )l<»,j-S{*)S(+)S(0*,j+ea,j  ) 
*C(83,  j)l3,  j+C(+)C(9)C(  +  ) 

♦siei.j+ei.i  )C(e=,  j  )  u,  j+c(*)S(e) 
*s(e=;J)i3j „ 

+S(*)C(+)C<9», ,+©»,.,) 

*C(03,J+e»,J ) IB*, j 

*c(ea,j+e*)J)ia»)j 

-s(*)S(4-)S(el,J+e=,J) 

*C(93, j+9», j ) la*, j 

+C(*)C<e)c<+)s<ei)J+eaE)J) 

*C(03,J+e»,J     )lm»,j 

+c(*)S(e)s<83,J+e«.,  j  )im»,  j+Pv 

R»,j,«    =    S(9)S(+)C(9i  ,  j+9=,  j  )  1=,  j 
+S(9)S(*)C(9l;J)l1;J 

+s<e)C(*)s<e1Jj+e»,J )iaiJ 
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+s(e)C(+)S(el]j)iXjJ 

+s<e)S(  +  )c<e1,  j  +6=^)0(9,^)1=,  „ 

+S(e)C(+)S(ei,J+0»,J)c(8wJJi)i»,j 

-0(9)3(0,^)13^ 
*C(9=5,  j+6»,  j)lm»,  j 

+s(0)C(  +  )S(91)J+e=,J) 

*C(83,  j+e»,  j  )lnu,j 

Taking   the   time  derivative   of    each   of   the   R's    gives, 
A1.5  R»,4,«    =   <t>a±  ,  j  ,  »+9bi  ,  j  ,  M+<t>c»  ,  j  ,~ 

+9=,  jdi.j  ,  «+93,  jei.i,  _+9.»,  j  f  i  ,  j  ,  -+P» 
Ei ,  j  i  v    =   4>ai,  j,y+8bi,  j.y+'I'Ci,  j,y 

+9= ,  j  d»  ,  j  ,  v  +93 ,  j  ei  ,  j  ,  v  +9» ,  j  1 1  ,  j  ,  v  +F'v 
Ri. .  j  ,  -    =  *ai  ,  j  ,  >  +6bi  ,  j ,»  +'t'ot  ,  j  »« 

+9=  ,jdi,j,.  +93 ,  j  ei  ,  j  ,  ■  +9»  ,ifi,j,r  +P« 
Where, 

i    =   2,    3,     4 


and   where, 


-S(«t> )  C(++e*  ,  j  +9=,  j  )  laa,  j 

-c(*)c<8)s<++ex,J)i»)J 

b=,j,„    =    SCt>)S(9)S(  +  +9i,  j+9=.  j  )lm=,  j 

+S(«t>)S(9)S(  +  +91,  j)l»,., 
c=.j,„    =    -C(*)S(  +  +8x  .  j+9=,  j  )  lm=,  j 
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d^.j 


b= , j , v 


da,  „ 


f=;J 


b=;J 


-S(*)C<e)C(++e1,J+e=.J)im=,J 

-C(*)S(++8»,i)l,,  j 

-s(*)C(e)C(*+elJJ)i1)J 

=   -C{*)S(++6xJ  j+e=,  j  )lm=t,j 

-s(<>)C(e)C<  +  +el,J+e=,  j  jiite^ 
=  o 
=  o 
=  C(0)C<++6i.,  j+e=,  j  lint,  j 

-s(<t>)C(e>s(  +  +6i,  j+e=,  j)im=,  j 

+c(«>)C(++el,a}u,Ji 

=  -C(*  >s<9)S(++9i ,  j  +e= ,  _>  ;  i>.n= ,  j 

-C(*)S{Q)S(++8xjJ)U,j 

v  =  -s(*)S('i'+ei ,  j+e=,  j )  im=,  j 
+c(«)C(e)C(++e1,  _,+e=..  jjirte,  j 

+C(*)C(9)C(*+6x,  j)lr,  j 

v-  =  c(*)C(e)C(++9i,  j+e3,  i)iaa,  j 

-S(0)S{++9t,  j+6=,  j  )  int.,  „ 

v      =      0 
v      =      0 

*  =    0 

.    =   C(9)S(  +  +e1 ,  j+9=,  j  )lm=,  j 

+c(9)S(++e1,  jju,  ^ 

*  =    S(9)C(  +  +9i,  i+9a,  -.  )ltte,  j 

+so)C(  +  +e1,  j  )iX)J 
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da, j,.    =    S(6)C(t+9i,  j+6=,  j  !lm=,  j 
8a,j,.    =    0 
fa, j,-    =    0 

te.j.H  =  -s(«)C(++9i,  j+e=,  j) 

*(C(93,  j)lite,  j+l=,  j) 
-C(*)C(6)S(*+9x,  j+9=,  j) 

*(C(93,.,Hnto,  j+i=,  j) 

-C(«)S(9)S(93,.,)l.m3,_, 

-s<*)C(++e»,„)u,J 
-c{*)C(e)S(++et,J)xt,J 

b»,J)M    =    S(*)S(6)S<  +  +9x,  j+9=,  j  ) 
*(C<93,  j  )  Into,  j+l=,  j) 
-S(*)C(9)S(93)J)lnto,  j 

+s(*)S(e)S(  +  +ex,  j)i»,  j 

=3,j,»    =    -C(*)S(*+9.  ,  j+9=,  j) 
*(C(83,J)lnto,  j+l=lJ) 

-S(*)C<8)c<++eijJ+e3tlJ) 

*(C(93,  j  )  Into,  j  +  1=,  j) 
-C(t>)S(++8x,J)l1)J1 

-s(<t>)C(8)C(++e»,J)iXsJ 
<i=,j,«  =  -c(0>s('i'+ex,J+ea,j) 

*(C(03,  ^  )  into,  j+1=,j) 
-S(*)C(9)C(  +  +9l,  j +9=,  j) 
*(C(e3,  j  )  lnv»,  j  +  b.  j  ) 
es,j,«    =    -C(*)C(++8x,  j+9=,  j  )S(93,  j  )lgts,  .. 

+S(<D)C(8)S<  +  +8x,  j +9=,  j)S(e3.  j  )  Into,  j 
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-S{*)S(e)C(e,;j)iItejJ 

f  3  ,  J   ..  „       =       0 

a=,_,,v    =   C(*)C{++9t,j+6a,j) 

*(C(93,_,)lite,  j  +  l=,  j) 
-S(<i>)C(9)S<  +  +9,,  _,+€>=,  ,) 

*(C(63,J)llIB,]+la,J) 

-s<*)S(9)S(e=5;J)iift3,  j 

+C<*)C(++©t,J)X»,4 

-s<*)C(e)s<++e1)J)i1L,J 

b3,J)V    =    -C(<P)S(9)S(  +  +6i,  j+9=,j) 

+C(4>)C(9)S(e3jJ)lm3,  j 

-c(<i>)S(e)S(++e*,  j)u,j 

C3,Jjv    =   -S(*)S(  +  +6i ,  j+8b»,  j) 

*(C(9=,  j)lltt3,  ^+1=,,,) 

+C(*)C(9)C(  +  +91,  J+9=,J) 
*(C(93,J)lia3,j  +  l=Jj) 

+C(*)C(9)C(  +  +91,  J)U,  „ 

d3>J,v  =  -s(«)s<+-fe» ,  j+9=,  j ) 

*<C(93,  _,  )  lm.-=,  j+l=,  j  ) 
+C(*)C(9)C(  +  +91,  J+9=,  j) 
*(C(93,  j)lni3,  j+1=,  j) 
e=,-i,y    =    -S(*)C{*+8»  ,  j  +9=,  4  )S(93,  j  )  lite,  j 

-C(*)C(e)S<  +  +9,  ,4+9=,  4  )S{9,,  j  !  Im, 
+C(-t>)S(8)C(93jJ)lite,  j 


yfi 


f=,J)y    =    0 
*»,j,,    =    0 

b=..j,«    =    C(6)S(  +  +6i,  j+es.j  )(C(e3)J)litt3,  j  +  l=,  j  ) 

+s(©>s{eSi,J,)inio)j+c<e)S<*+8l,J)it,j 

c^.j,^  =  scejcct+ex,  j+e=,  j  )<c(93,  j  )im.3,  j  +  i=,  j  5 
+S(e)C(  +  +e1,  j)ix,  j 

cU, _....,    =    S(e)C(  +  +6x,  j+e=,  j  )(C(63,  j  ilrtte,  j  +  1=,  j  ) 

e3;J.«  =  -s(e)S('t'+el,  j+e=,  j  )S(e3,  j  )inte,  j 

f  =  ,  J  ,  ■       =      0 

sw,j,„    =    -S(«)C(++9x,j+9»,j) 

*[C(83lJ+e.,J)lm«,J 

+c(e3)J  )i3,  j+i=,  j] 
-c(<>)C(e)s<++ex,J+ea)J) 

*[C(e3,j+e., j)im.,i 

-C(*)S(9)[S<83, j+6*, j)1b«, j 

-C(*)C(e)S(++9t>J)u,J 

b»,j,„  =  s(*)S(e)S(++e!, j+03, j ) 

*[C(9a>,j+e»,  j)lia»,j 
+C(Sa,  j  )  la,  j+la,  j  ] 

-s<*)C(6)[S(e3,  j+e.,  j)im»,  j 
+S(e3lJ) u,j] 
+s(*)S(9)s<  +  +ex,  J>il,J 
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cU, 


-*  j  H       — 


e-», . 


f»,j, 


*[C(83,  j+9»,  j)1bw,j 

+c(e3,J)i3,J  +  i=.  j  ] 
-s(*)C(e)C(++e»JJ,+e«,j) 
*[C(e3,j+e», jjim-.j 
+C(e3,.i)i3,  j  +  i=,  j] 
-c(0)S(++e»,  j)k,  j 
-s<«t>)C{e>c(++$»,.,)u,J 

*[C(e3,  j+e.,  jUbu.j 

+  0(63^)13^  +  1=,    j] 

-s(«)C(e)C('('+ex,  J+e=;  j) 

*[C(03, j+e», jilmA.j 
*[S(03,  *+Q»,  i)lni.,i 

+s(*)c<e)S(++e»,  j+e=..  j) 
*[S(63,  j+e.,  j  )im»,i 

+  S<93,J)l3,_>] 

-s(*)S{e)[C(e»,J+e»>J)im«)j 
+c(e»,J)i!,,J] 

-C(*)C(  +  +9i,  j+0=,  j  )S(93,  j+e»,  j  )lm»,  j 

+s(<o)C(e)S(  +  +ei,  J+e=,  .,; 

*S(63, j+e», j ) lm*, j 

-s(* )  s<e )  c(03,  j  +e. ..  j )  im« ,  j 
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a*,j,v  =  c(*)C(++ex,  j+e=,  j ) 
*[C(e3,J+e»,  jjinu,  j 

+  C(93,_.)l3,   J+l=,_,] 

-S(*)C(e)S<++e*,j+e»,.i) 

-s(<t>)s<e)  [S(93,  j+e»,  j)im«,  j 

+S<6a,4jl<,,j] 

+c<*)C(++e»,J)i»,J 
-S(«t>)c<©)S<++et,J,)uJj 

b»;J,v  =  -c(*)S(e)S(++9i,  j+e=.,  j) 

*[C(93, j+6», j)lm», j 
+C<9»,j)l»,J+X»,  j] 
+C<*)C(9)[S<©=.)j+e»,J)lm»,j 

ts(e«,i)u,j] 
-C(0)s<e)S(++e,.)J)u,J 

c,j,y  =  -s(<t>)S(++ex,  j+e=,  j) 

*CC(8»,j+9»,  J)lm*,J 
+C{©3,j)l3,j  +  la>J] 

+c<4>)C(0)C(++e»JJ+e»,J) 

*[C(e»jJ+9»,j)lm«,j 

+C{e»)J)l»,J+la!,J] 

-S(*)S(  +  +e1.  jjix.j 
+C(t>  )C<e)C(  +  +9x,  j)ii,  j 

d»,j,v    =    -S{*  )S(*+8i  ,  x+9=,  i  ) 
*[C(8,,J+6.,J|liii.,1 


101 


+C(e3,.,)lw,J+la,4] 

+c(*)C(e)C(  +  +e,,  J+e=,  j) 

*[C(e3,  j+e»,  *)lnu,  j 

+C(6i=,  j)l3,  j+la,  j] 

e»,J>v   =   -S(*)C(>t'+9i,  j+03,  j  ) 

*[S<93,  j+e»,  j)lm»,  j 

-C(*)C(e)S(  +  +6i,  j+9=,  j) 

*[S(e3,J+e»,  s)im~,j 
+S(e»,J)i»)j] 

+c(<t>)S(e)[C(e3jJ+e^,  jjim^.j 

+C(8,,J)l3,J] 

-c(4>)C(e)s<  +  +e1,  J+e=,  j) 

a*.j,«    =    0 

b^.j,,:  =  c(e)s<++ei,  j+©»,  j  jcccqs,  j+e», ..  )im»,  j 

+S(9)  [S(9=,  j+9»,  iUa*,  itS(83,  j  )1»,  _,] 

+c(e)5(t+e,lJ)i,,J 

0»1J(J«    =    S(9)C(  +  +9i.  j+9=.  j  )[C(93,  j+9«,  j  )lm.,  „ 
+0(93^)13^+1=,  a] 

+s<9)C(  +  +e,.,  j)u,j 

d»,j,»    s    S(9)C(  +  +9x,  j+9=,  j  )[C(93,  j +9»  ,  j  )lnu.  j 

+  0(93,   J   )   13.^+1=,    J   J 
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e-»,j,*  =  -s(e)S('t'+e1 ,  j +e=,  j  ) 

*[S(63.J+8.,J)liii«,J 
+S(8»,j)1»,j] 

-C(8)[C(83,J+e.,J)lm,,i+C(8,,i)l3,J] 

t+,*,m  =   -s<9)s(++ei,j+e=,  j)s<63,  j+e«,j)iro»,j 
-C(e)C(e3,  j+e*,,.  )1b»,j 

The  total  kinetic  energy,  K,  of  the  system  is  now 
written  as, 

A1.6  K   =    l/2Mb<P..  =  +rV=+P.=  > 

+  1/2  (   Ix  ,  OW3«   ,  O  +  Iv  ,  OWZy  ,  0  +  I«  ,  Ot>JZ«:   ,  O  ) 
6  4 

+  1/2  £       E  {m»,j[(B»,J,,M)*+<B»,jJv)a 

j=l    i=2      . 

+  (Ri,J,«)=] 

+  I  A   ,  J  ,  v  (  Wl  —  X  ,   J   »  V  )  a 

+  Ii  ,  J  ,  K  (Wl-l  ,   J   j  ■   )=  ]  } 

And  the  potential  energy,  P,  is, 

6    4 
A1.7    P  =  MbGP,+  E   E  M»,jGR»,j,, 

j=l  i=2 

The    Lagrangian   L    is    then, 

A1.8  L   =    l/2Mb(P«  =+Pv=+pI  =  -2GP«) 

+  1/2  (  Ix  ,  ow=m  ,o+Iy  ,  ow'y  ,  o  +  I*  ,  oUS*  ..  o  ) 

8      4 

+  1/2   E        E   {au  .  »[(H»  ,  _■.«  )  =  +  (R±  ,  j  .  v)  = 
j=l    i=2      . 

+  <Ri  ...  ,.)  =  -2GRi  ,j,.] 

+  [    I  »     ,    J    ,M    (Ul-l    ,    J    ,   «     }  = 
+  Il     ,   J.y(l,>l-1.    J    ,y)2 
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The  generalized  coordinates  are  defined  as, 

A1.9      qi  =  P„  q-p  =  9»,i  qj-7  =  6t»,« 

q=  =  Pv  qio  =9=,=  qin  =  9»,» 

qr=  =  P«  qn  =  9*,=  Qit  =  9z,a 

1»  =  ♦  qi  z  =  9» ,  =  <bo  =  6s ,  a 

q=  =  9  qis  =  9=,^  q=i  =  9»,b 

q«.  =  +  qx ■»  =  93  ,  3  q=z  =  6=  ,  a 

qr  =  62,1  qio  =  9* ,  a  q=3  =  9s,a 

?s  =  63,1  qi*  =  82,*  q=»  =  9»  , «, 

By  defining  p,  q,  and  r  as  the  support  legs  and 

requiring  the  endpoints  of  the  support  legs  to  stay  at  a 

fixed  location  in  the  world  coordinate  system,  the 

constraint  equations  become  identical  to  those  in  Section 

2-1. 

Lagranges  equations  then  become, 

9 

3q±  I       9q±  m=l 

i=    1, 2,  .  .  .  ,  24    and, 


ALIO        d_(9Lj    -21         =   E   X.am.i+T. 
dt\3cu;       3qi 


24 

E  a™,  1  4i  +a™.  t   =   0 
i=l 

m   =    1,2,  ...  ,9 

Where  qi  is  defined  in  Equation  Al . 9  and  Km,  a>,i,  am,*, 

and  Ti  are  defined  in  Section  2.1. 

To  begin  taking  the  partial  of  L  with  respect  to  4i ,  L 

is  broken  down  into  components  and  the  partials  of  those 

components  with  respect  to  q4  are  taken. 


Al.  11    8P«  =  0  3PM  = 

9P-  9P„ 
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2£^  =  0 

3P_ 

3R=,  i  ..    =   0 
3P„ 


3P-a    = 


3P- 


=    0 


3(R=,  j,-  If    =    2[P»+*a=,  j.x+etta,  j,, 


3[R 
3K 


+te,i,» +a= , jda , j , » ] 


31R=.  ,.,!'    =    0 
3P- 


3P» 


3  (  Rg .  1 .  - i ; 
3P- 


+*c3,j,„  +03,  jtb.j  ,-+63,  ies,i>>] 


3_L&l, 


3P„ 


15   =    0 


9(R».  ....)=    =    0 
3P« 


3(R».  ,.„  )J    =    2[p_+*a»,  j,»+eb»,  _.,„ 
3P„ 

**c.,j„  +e= ,  j  d» ..  j  ,  m  +e3 ,  j 


3(R» 

=  0 

3P„ 

3u2„ 
3P- 

0 

=    0 

3u2,  . 
3P* 

0 

=    0 

3  ^COi   , 

,*■)= 

=  0 

3P„ 

3  ( ua . 
8P» 

J 

,»)  = 

=  0 

3(..J=. 

..  ;= 

=0 

3P„ 

3(E. 

,  j  ,  *  )  =    =    0 

3P^ 

9t*ja~ 

o    =    0 

3P„ 

9(uii 

.*..)*    =    0 

3P„ 

3  Jwi 

,  j  ,  *  "i  =    =    0 

3P„ 

3  Cu= 

J.„)a    =    0 

3P» 

3fw, 

..  ..  )a    =    0 

3  P., 
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S?(<~t3,    1  ,„)a     =     0 

8P„ 


3P„ 


Al.  12         3P,    =    0 
3PV 


3P„J    =    0 
3Py 


8PV= 
3PV 

=   2Py 

3R=._, 

■  ^    =   i 

3PV 

3R^,_, 

..    =   i 

3Py 

3(R». 

J   .  V  )  = 

9P.=    =  0 
9Fv 


3(R=.J.-1=    =    0 
3JV 


2[Pv+*a=,  j,y+9b=,  j,y 
3PV 

++0= , j , y +6= ,jda,j,y] 


3JH.3.  j.,)3    =    0 
3PV 


3  ( R3 ,  i  ...  '1  g    =    2[P¥+*&,,  j  ,  v+9t«,  j  ,v 
3Py 

+<&a»,  j,v+0=,  jd3,  j  ,ytfe,jej,  J  ,y] 


8(R,.  ,..',=    =    0 
3PV 


9(R».-..m)z    =    0 
3PV 


3(R» Lg    =    2[Pv+$a»,  j,v+9b»,  j  .  v 

3PV 

++c.  ,  j,v+9=,  id.,j  ,  y+fe,  j 


*e»,  j,y+6.,  jf«,iiy] 


9<r».  tJJaf  =  0 

3PV 

8uJw  ,  iy      =      0 


3PV 


3PV 


34- 
3PV 


I*    =   0 


8Py 

in 

- 

0 

&J», 

= 

0 

3PV 

8  («, 

-V. 

',  = 

3PV 

3«VJ= 

1  j 

•'  = 

3PV 
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ai 


^a,  j.vi- 


3PV 

3PV 

9  ( Ml .  i  ■  i  1 =    -    0 
3PV 


3PV 


Al.  13        SPs.   =   0 
3  P. 

3PV=    =   0 
3P- 


3P>,2 
3P* 

=   0 

3F*  = 
3P- 

=   2P, 

3P« 


3R*,, 
3P* 


3fe.j  .*    =    0 
3P« 

3(R=.  .■.■<)=    =    0 
3P« 


3  ( Rj ,  | . .,  i  =    -    (j 

3  f  R= .  ...)z   =   2[P,+eb=.J,.+icI,i.I+e2.Jd=,J..] 
3P* 


aiPa^j^a^  =  o 

3P, 


3fR3,  j  ,vP    =    0 
3P^ 


ai&^jL^IS   =    2[P. +6b3.  j.^+'i-c,  ,  _.,,+«=,  jd.,,  j, 
3P* 


2IR»,  j...  i=  =  0 
9P. 


9F, 


S4&^_j_^t^  =   2[P«+9b»,_i,,++c»,j,. 

9P- 

+3=a  ,  j  d<*  ,  j  ,  «  +03  ,  J  ©5B  ,  j  ; 
+©»,  jf»,  j,,] 


=    0 


9P. 


3P. 
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9  (uji 


=    0 


-A.Z 


9P. 
3P» 


3(m  ,  ■  ,„  )-    =    0 
9?. 

9  Uii .  j  ■  -  )a   =    0 


3  ^uta  , 


a=  =  o 


3  p. 


9(<-Js_, 


1=    =    0 


9P> 

Al.  14        3P*    =   0 
34> 

3JLi   =   0 
3* 

3Kz,  j  ,*    =    0 
3* 

3R».  ..  ,.    =    0 
3* 


3F, 

3fto=,  <  , .,  g    =    0 
9P. 

sLLi&Li  <  ■-  1=   -   0 

3P« 


3P« 

3P»S    =   0 
3* 

3?^    =   0 
3* 

3R-3 
3* 


=    0 


3(i= 


■  )=   =   2a=,  j ,» [*a=, j ,« +6b=. j ,» 
3* 

+io»,j„  +6=  ,jd2,i,»  +P„  ] 

9.1  fiz.  j.^P   =   2az,j,v[te,j,y+6b=,j,y 
3<j> 

++0=  ,  j  ,  y  +9= .,  j  d3 ,  j  ,  v  +PV  ] 

3  ( Bs- .  ..,')=    =    0 
30 

9,(Rs.  .  .  -  )z    =    2a-5,  j  .-  [*as.  j  .x+ettj,  j  .- 
3* 

+  +C55,   j,„+9=,   l!b,J„ 

+9^ ,  j  (fa,,  j ,  „  +p„  ] 

3  ( fig  ■  ..-I1    =    2a,3,  j  .  v[*ar  ,  j  ,  v+flfe.  j  ,  v 
3<t> 

++Cs,  j  ,v+e=,  j  da,  j  ,  w 

+  93  ,lfe,j,v  +Py  ] 
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3<t> 

9-{-B».  < .-  )=   =   2a»,  j  ,«  [ia«,  j  ,„+6b»,  j  ,« 
9* 

+93 ,  j  &» ,  j  ,  „  +e<  ,jf4,j,»  +p„  ] 

3(R».  i.^)2   =    2a»  .  j  ,  ».[*a*,  j  ,y+eb»,  j  ,„ 
3* 

+*03,j,y+8i,jd«,j,y 


+©3,  je,,j,«+e.,jf«,J)ytPy] 


91R^ 
3* 


a^-.o  =  2S<9)S{*)t*s(e)S{*)+9C(+n 

3* 

a^i^o  =  2s<e)C(+)[*s<9)C(+)-es('i')] 
a* 

auis^a  =   2C(9)[*C(93++] 

3* 

Stjiea .  j  .- 1^  =  2S<e)S('i'+e1 ,  j+e=,  j  > 

3* 

*[is(9)s<++e1,J+e=, j ) 
+9C(++ei,  j+e^.j  5] 

3(u,  .  ,...)J    =    2S(9)C(++9i  ,  _■+«=,  j) 
3* 

*[*S(9)C(  +  +91, j +9=, j  ) 

-9S(  +  +9i,  j +9=..  j)] 

9(m-  «■-)  =    =   2C(9)[*C(9)++] 

3* 

3-iv-s,  j,-  g    =   2[S<9)S(++9»,J+9=,J5C(93,j) 

3* 

-C(9)S(93lJ)] 

*{*CS(6)S(++9i, j+e=, j ) 
*C<83, j)-C(9)S(9=, j ) ] 
+6C('('+9i  ,  j +9=,  j  )C(9^,  j  5 


109 


3* 


3<u 


-+S(e»,j )-e=, jS(93, j )} 

+0(0)0(9,, ^ )] 

+ec('t'+el,  j+e=,  j  )S(e»,  ..  ; 
++0(03,  J)+e=..  jcie^.j)} 
§4^=  ■ .» . . )  =  =  2S(6)C('i'+e1,  j+e=,  j) 
*[*s(e)C(-v+e1,  J+e=,  j  j 

2[S(9)S('l'+9l  ,  j+9=,  j  )C(8T,j+e«,  j  ) 

-c<e)S(9»,j+e», j )] 

*C(e=:,J+e^;  j  ) 
-c(e)S(e3)J+e», j>] 

+eC(++9T, j +9=, j )C(9,, j+8»,j } 

-+s(e3,  j  +e»,  j>-e»,iS<e=i,  4+e»,  .,  )> 

2[S<6)S(++9x .4+9=, j )S(93, j+9.,  _,  ) 
+0(9)0(9,,  j+9»,  j  5] 

*{*CS(9)S(++e1,  j+e=,  j) 

*S(9,,J+9.,J  ) 

+0(9)0(9,, 4+©»,j)] 

+eC{<l'+8t,  j +9=,  j  )3(9,,  j +9^,  j  ) 

++0(9,,  x+9.,  j  )+93,  jC(9,,  j+9»,  j  )  } 

9  ( cj, ,  . . .  ) ^    =    2S(9  )C(  +  +9i  .  j +9=  ,  j  i 
9* 


9_ius^ 
9* 
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Al. 15        3P*    =   0 
86 


*[*s(e)C<++e1,J+e=]J> 

-es  ( + +e  x ,  j  +e= ,  j )  +e3 ,  j  +e» ,  j 


3_E*f.   =   0 
36 

3R=...  .,    =    0 
36 

3EU.  ,  .,    =   0 
36 


3  f.~    =    0 
36 

ag? ,  j  ,  -  =  o 


3(R=.  ,  .-  )  =    =    2b=,  j  , «  [6b=,  j  ,„+<l>a2,  j  ,  „ 
36 

++c= ,  j ,  „  +e= ,  j  d= ,  j  ,  „  +p„  ] 

3(R=.„,„)  =    =    2b=,  j  ,v[6b=,  j  ,v+<ia=.  J;y 
36 

++oa  ,  J  ,  v  +6= ,  J  d=  ,  J  ,  y  +  Py  ] 

3J  R=  ,  j  , .  )  *    =    2b=,J,.[9b=,J.I++c=,J.I 
36 

+9=  ,  jda ,  j  , .  +F.  ] 

3  ( R» .  -....')=    =    2b.3.  j  ,„  [9bi,  j  ,.+*as,  j,„ 
36 

+63  ,je4,j,,  +F„  ] 

3  (  R3  .  j  ,  „  i  =    =    2b3,J,y[9fe,J../+*aT,J,y 
36 

+'t,C=s,  j,  v+6=,  j  da,  J  ,y 
+63  ,  j  S3  ,  J  ,  v  +P„  ] 

3  ( Sa  ■  inP   =   2b3 ,  j  .  ■  [Sbo ,  j  ,  «+*03,  j  , . 
36 

+6=  ,  jd»  ,  A  ,  m  +63  ,  J  »»,  J  ,  »  +P.  ] 

3(  R»  .  .  .  _  )=    =    2b»  ,  j  ,  m  [6b»  ,  j  ,  „  +<t>a»  .  j  .  „ 
36 

++c»  ,  j  ,„  +6=,  jd»,  j  ,  „ 

+63 ,  j  e»,  j ,  x  +6»  ,jf4,j,,  +P„  ] 
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SUU.  j,~)z   =   2b-.,  Jiv[8b»,  j  .v+*a».  J)V, 
++C-. ,  j  ,  v+8=.,  jd»,  j  ,  v 

+9=  ,  J  &»  ,  J  ,  V  +9-»  ,jf»,J,y  +Py  ] 

+8= ,  j  d-» ,  j  ,  ■  +9rs ,  j  e* ,  j  ,  ■ 

+9-,,  jf»,  j,.+P„3 

3ya^  . o    =    2C('(')[9C(  +  )+*S(9)S(+)] 
99 

a^-.o  =  2S{+)ces<+5-*s(ejc(+>] 

98 

8uiZ,   ,  <->      =      0 

99 

31  wi  ,  .  ,-  g    =    ZCt'V+ei .  j+9=,  j  )  t9C(  +  +9i  ,  j+8=,  j  ; 
39 

+«s<e)S(')'+91  ,j+ej»,j)3 

aiaa  ■  L^aeJg    =    2S(  +  +9i,  j+9z,  j  )  [8S('I<+91  .  j+fli,  j  ; 
98 

+*S(9)C(«n-e1,  j+e=;J )] 

3(uji  .  . ,,  )=    =    0 
38 


38 


{torn.  ,.-!»    =    2C(  +  +9x,  j+9=,  ,)C(83,  j  j 
8 

*{eC(++9i, J+8=, j )C<83,j ) 

+*[S(e)s<++©»,J+ea,J)c(e»)j) 

-C(9)S(9=jJ)] 
-♦SOa.j  )-8=,  jS(8s,  j  )} 
==   =    2C(++6i,  j+8=,  j  )S(93,  j  ) 

*{8C(++6i , j+©a, j )S(9s, j ) 

+*[S(e)S<++ei,  j+e=,  j  ssie,^ ) 

+C(8)C(9,;J)] 

++C(93jJ )+e=, jCtfe.j ) > 


38 
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90 

-4>s<e)c<++e1,j+e3=lJ)-9a)J] 

aihSL.  '■-  >^   =   2CC*+6x,Ji+®a,4)C(9»,j+e»,j) 

38 

*{ec(++e1,J+e=,J  >C(e»,j+e»,j ) 
+*[S(e)s<++elI j+e=, j )c<©w,j+e»,j ) 

-C(6)S(9,..  j+6»,  j  )  ] 

9(.-^.  ,...1=   =   2C(++ei,j+6a1j)S(9»,j+e*1J.) 

98 

*{ec(++8i,j+fe, j )S<&B,j+e»,j ) 
+<i[S(e)S('t'+81,J+e=,  jjso^.j+e^,  j) 

9(i^3,  ..,  )=    =    2S(  +  +9i,  j+8=,  j  J  [8S(  +  +9i  ,  j+9=.  j  ) 
98 

-4s(e)C<++9» ,  j+es.j) 


-6a 


A1.16        3P. 


3  9P„Z    =   0 

3^  9+ 

2?*f.   =   0  9_B^   =   0 

9+  9^ 


9-Sa-,  «  ■-    =   0  9R- 


3R=.  <  ■-    =   0 


-3.-1  .  g 


a*  9* 


9+ 

+9b=  ,  j  ,  „  +9= ,  j  ,1= ,  j  ,  „  +P„  ] 

9  ( R=  ■  < ... )  ~    =    2c=  ,  j  .  v  [*oa  .  j  .  v+*a=  .  j  .  v 
9  + 

+9b=  ,  j  ,  „  +9=  ,  „  d=  ,  j  ,  v  +  PV  ] 
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8+ 


9(fe».  1 .  -  )■ 
3+ 


9<r^ y- 

3* 


9+ 


si 


9+ 


2c*,j,. [*Oa,4,«4©ba, j,. 

+e= ,  j  d= ,  j , .  +p«  ] 

2cs ,  j  ,  «  [+C3 ,  j  ,  x  +'j>a3 ,  j  ,  « 
+8b3 ,  j  ,  „  +6=  ,jds,j,« 
+93,  je=,  i,«4-pM] 
2c3,j,v[^C3,j,y+<ia-3Jj,y 

+9b3  ,   J   ,  y  +82  ,jCU,J,y 

+9:3,  J  (53,  j  ,ytPv] 

2C*  ,    J    ,  „    [  +  C.  ,    J   ,  M  +*a»  ,   J   ,  H 

+9b»,  J]K+«2,  jcU,  j  ,„ 

+9= ,  j  e» ,  j  ,  »  +8* ..  j  f » ,  j  ,  m  +P„  ] 

2c. ,  j  ,  vOc. .  j  ,  v+*a»,  j  ,  v 

+9b.  ,  j  ,  „  +e= , ad,,  j ,  v 

+83 ,j«t,Jt.  +6. ,  j  f « ,  j  .  v  +Pv.  ] 

2o«,j,,[to»,j  ,  «+9b» ,  j  , « 

+93 ,  j  d. ,  j  ,  *  +63 ,  j  e. ,  j  ,  x 

+9*  ,Jf4,i,«+PI] 


8+ 


9yz^ ■ o    =    0 
3$ 


SsjSs^s.  =   2[*+*C(6)] 
9* 


3* 
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9<|/ 

3^=,,,- 
3+ 


3+ 


3>V 


=  2[++«c(e)+e=;J] 

=    2S(83, j ){is(9*, j ) 

-c(9)s<e3,  J)]-ec(++el,  „+«=,., ) 

■4[S(e>S{++ei,j+eai)J  >s(9»,j  j 

+c(8)C(e3,  j  )]+ec{<t-+9T,  j+e=,  j  ) 

*S(63, j)+6=, jC(93,j)} 
0 


3  (  U13  ■     ,    ,  „    )  - 


2S(e3,  j+e»,  j  ){<i/s(9s,  j+e»;  j> 

-c(6)s<e3, j+e», j)] 
-ec(++e1(  j+e=,  j  )C<3,  j+9»,  j) 

+9=, i3(63,jt9.,j )} 
2C{9:=,  j+8»,  j  )  {^C<9»,  j+6»  ,  j  ) 
+4>[S(8)S(++8i,J+8*,J  )S(93,  j+8a,  j  ) 
+0(0)0(83,^+8.^)  ] 

+ec(++e» ,  j+e=J  j  )s{8»,  j +e»,  j  ) 

+9=, jC(9»,j+6», j ) } 
0 


Al.  17         3P,    =    o 
38=,  j 

3PY^       =      C 

38= ,  _, 


3P„  =    = 
39=,  j 

0 

8P«=    = 

0 

39=,  j 

IIS 


36=,. 


as*..!..,  =  o 
30= ,  j 

3fc».  i  ■  ■■  =  o 
ae3jJ 


3a3)J   * 


3(R=.  ,.-  )  =    =    2da,  _,,„  [9=,  jit.j  ,»+»8a,  j  ,„ 
+9b= ,  j  ,  „  +ic= ,  j  ,  x  +Pv  ] 
=   2d=,  j  ,  y  [9=,  jd=,  j  ,  v+'ia=,  j  ,  v 

+9b=,  J,v++C»,4,v+Pw] 

=   2d=, j ,! [9=, jdz, j ,«+6b=, j ,- 

+>t'C=,  i,«+P»] 

=   24-3,  j  ,»  [6=,  jd.3,  j,»+*a3,j,« 

+9b3,  J  ,  „  ++C3,  J,_ 

+63  ,jto,j„  +F„  ] 
=   2d3 ,  j  ,  v  [  63 ,  j  da ,  j  ,  y  +*  a-r ,  j  .  y 

+9b3  ,  j  ,  y++G»,  4,  v 

+  93  ,   J  S3  ,    J  ,  v  +  P„  ] 

=   2dr,j,.[9i,jii3,j,,+eb3,j,, 

+  i03  ,   J   ,  ,=   +03  ,   J  S3  ,   J    ,  ,  +  P*  ] 

=   2d» ,  j  , .« [  9= ,  j  d» ,  j  ,  „  +<i  a» ,  j  , » 

+9b.*  ,j,«4c4,j)M 

+©3  ,  j  e»  .  j  ,  m  +fl-»  ,  j  f » ,  j  .  K  +P„  1 
=    2d* ,  j  ,  y  [  9= ,  j  d-*  ,j,y  +0  a-* ,  j  ,  v 

+9b»  ,  J  ,  y+*a»  ,  J,y 

+©3 ,  j  &» ,  j ,  „  +e»  ,jft,],y  +pv  ] 

=    2cU,j,«t8=,jd»,j,.+9b»,4,« 
+9-»,  jf»,  j  ,,)■?,  ] 


3     (     R=     ,         .      ,     y     ) 

39=,  j 


39=,  j 


a-i-Ss  ■  1  ■  - 1 

39=,  _, 


3(BU, 
30=,  j 


34^j_^i 

39=.  j 


39=,  j 


39= ,  j 


34^ 
30=.^ 
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a^s.  ,o  =  o 

96  =  ,  j 

3u^« . „    =    0 
36= ,  j 

3(ujt  .  ,  .  v)  =    = 

:    0 

36=..  j 

3<>J=v  .  O      -      0 

36=,  j 

9{m,.....)z    =    0 

36= ,  a 


3ju>i  .  ,  ..  )=    -    2[9=.  j+4>C(e)++] 
36=,  j 

9{w>.  ,■-)  =    =    2S(e=5,J){9=,JS(6^,  j  ) 
30=,  j 

-* [ S (6 ) S ( +  +9x , j  +9=, j )C(6», j ) 

-C(e)S(e3,j)] 
-ec(++©x,  j+9=, j  >c<e»,j ) 

9fw=.  . ...  If    =    2C(63,  j  >{9=.  jC(63,  j  ) 

3e=)J 

+«[s<e)S{++ex,j+e«,j )s(e^,J) 

+ec(++e» ,  j+e=,  j  )s<e=,  j  j 

++C(83, j)} 
§4^a-  -■■-  )=    =    0 

a-i^Sj-ju-.!^   =    2S(65,J+9«.J){e=.j8(93,i+94,J) 
96=,  j 

-<t>[S(9)s<++e1,  j+e=,  j  )C(63,j+e»,j  s 

-c<8)S(e=,,J+»,4}] 

-ec<++8i, j+e=, j )C(9», j+e«,j ) 

Sliaa  .  j  .  ~  i g    =    2C(93,  j+9»,  j  ){9=.  jC(03,  j  +6«,  j  ) 
96=,  j 

+*CS(e)S<++9i , j+e=, j )S(9^, j+e». j ) 
+C(e)c<es,j+»,J)] 
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+ec( *+e» , j  +e= , j ; see, , j+e» , j 


9  (  U>=  .  . . ,  )  -  =  o 
36=,  j 


A1.18    3P. 
36= 


3P^  =  0 
363jJ 

3R=.  ..-  =  0  3B,.  .  .,  =  0 

36,,., 

3R~,j,*  =  0  8(Ra...  .,  1=  =  0 

9©s,j  36,, j 

3IBa.,j,v)  =  =  0  3(R=.  ...  )a  =  0 

36=.,  j  90,,  d 

3f  R,,  . ,  -  )=  =  Z&3,j,,[93.4e3,j.K4a3,j,» 
363,  j 

+6b3,  j  ,  „  h-^c,  ,  j  ,„ 

+9»,  ids,  j,«+P»] 

3_{R,,  .,.,)  =   =   2e,,  j  ,  y[fe,  ifti,  j  ,  v+<i>st3,  j  , „ 
36,,., 

+6b,  ,  j  ,v++Cb,  j,  v 

+6=,  4ds,  J-v+Ps-] 

3  ( Sa ,  i ,  „  j  -   =  2e»,  j,«cea>,  je»,j,«+0b»,  j  ,r 

36, ,  j 


++©»,  j ,«  +6=  ,jd,,j,,  +P*  ] 


36,,  j 

+9b*  ,  j  ,  „  +<]'  e*  ,  j  ,  „ 

+6=,  id*  ,  j  ,  »  +6«,  j  f»,  j  ,  „  +p„ 

S4-&*--  j  i  v)=   =   2e»,  j  ,  v  [6,,  j  e-».  j  ,y+<t>a»,  j  , 
36,^ 

+6b,  ,  j  , V++C»,  j  ,„ 

+6= ,  j  dU ,  j  ,  v  +9*  .•  if»,j,vtPv 
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Sl-S*  ,  * .  „  ) z    =    2e.,J,I[93,je.,J,.+6b.,j., 

++G-* ,  j , -  +9=  ,id»,j,i 

+84,lf»,4,.4.] 


303,  J 


3cj2^  .  o    =    0 

303,  J 


3y=.._, 


39, 


9('-»  ■  <  •»  )=    =    0 

363,  J 


3(m  ,  j  ,v)a    =    0 

303,  J 


3 fui , j  ,« 

303,  J 


303,  J 


9  (  <.j=  ■  i  ,  ^  ) s    =    0 
303 ,  j 


303,  J 

-es(++e1,J+0=,J)] 


303  ,  J 


9(PJ3.     Ujtjg       =      0 
303  ,  J 


3  j  i*-. .  , ,  .  )  g    =    2[93.  J+*S(9)C('t'+0i,  j+9=,  j  ) 
303,  J 

-eS(*+0i  ,  j+0=,  j  )+9»,  j] 


Al.  19         3Ps_   =    0 
39»,  j 

a|*f  =  0 

39»,  j 

affa  -..>  =  0 

30«,  j 


3P>.=    =    0 
30-.,  j 

9P>  =    =   0 
3«« ,  j 

3R3  ■  „  .  *    =    0 
39-,  j 


39»,  j 


3JK*,, 


30- ,  j 


3(B=. j.« )= 
39-»,  j 


3(R3,  ,1  ,„  )2   =   0 
39- ,  _■ 


9  (  P-3 


)=    =    0 


9e»,  j 
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36*,., 

3(R*.  ,.■.)  =  =  2f.,J,H[9.,Jf4,J,.+ia«,4.» 
36*,  j 

+9b»,  j,«4tB«,j,>< 

+8=,  jd.,  j,»+fe,  je.,  j  ,m+P».  ] 

9  (  B>  ■  i  ■  - 1 J  =  2f » ,  j  ,  v  [9» ,  j  f ■•  1 4  ,  v+* a«  j  j  ,  v 

39* ,  j         . 

+9b*,  j  ,  v++&»  ,  j  ,  y. 

+83,  jd.,  j.M+e^,  iSa,  j,y+Pv] 

34JS*,  j,.)z  =  zf4,J,.te..jf4,J,,+9b,.j,, 

38*,., 

++c» ,  j , « +e=  ,jd«,j,. 

+83  ,j9l,i,.  +  P*  ] 

=  0  9u>=~ .  o  =  0 

39.,  j 

=  0  8(m.i..  ^J  =  0 

86*,  j 

3{^,  Jnr)  =  =  0  3jwi.J..)a  =  0 

39»,j  36*, j 

3_(^z,  j.M)=  =  0  3juz,  .  ..,  )J  =  0 

39* , j  30, , j 

ajJtStj  1  IT  )~   =   0  3(M3..3  ,,  1  =   =   0 

9Q" , J  36* , j 

9-ii^.  j.»)g  =  0 
36* ,  j 

MltlSL,  I  ,  -  )z  =  2[6*,  J+'i>S(9)C('J'+9i  ,  _■+«=,  j  i 
38* ,  j 

-8S('('+61  ,  j+9=,  j  j+e,,  j] 
This  completes  the  partials  of  the  components  of  L 
with  respect  to  the  4»'s.   The  steps  yet  to  be  accomplished 
are,  a)  taking  the  time  derivatives  of  the  previous 
partials,  b)  taking  the  partial  of  L  with  respect  to  the 
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Ii's,  and  c)  taking  the  partials  of  each  of  the  constraint 
equations  with  respect  to  the  q± ' s  and  with  respect  to 
time. 

With  all  the  neccessary  partial  derivatives  taken, 
Lagrange's  equations  can  then  be  written  in  the  form  of 
Equation  Al. 10. 
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APPENDIX  II 


The  following  is  a  listing  of  the  simulation  program 
and  the  input  data  files  it  uses. 
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c 

c 
c 

C        THE  MAIN  ROUTINE  IS  USED  TO  SET  INITIAL  VALUES* 
C        CONTROL  THE  TIMING  LOOPS,  ANti  CALL  THE  NECCESSAR1; 
C        SUBROUTINES. 


CHARACTERS©  ID 

REAL  INCRf IX0jIY0>IZ0jL(4»6) »K<4»6) >LM<4j6) 

REAL  KMC4) . KD >KP > KI > MB 7 IX < 4 • 6 ) r IY(4i6) » IZ< 4f 6) 

REAL  KPX<0!3)  >KPY<0J3)  >KPZ<0!3)  ,KPMI<0?3) 

REAL  KTHETB<0)3>»KPSI<053) 

INTEGER  LP»LQ>LR»AP»AQ»AR 

PARAMETER  cy«17» IPQINT»3> ' 

DIMENSION  THETA<4f6)  ?DTHETA<4,6)  ,RQ<125  ,00(6,65 

DIMENSION  QPX(U5  >QPY<V>  ,QPZ(U>  .QPHK'J)  ,QTHE7B(U> 

DIMENSION  QPSKV)  7QEX(6,U5  ,QEY(6<'vii  »QEZf  6*U)  i  T  C  4  7  6  5 

DIMENSION  R(4»6) >BR<4»6) > DDR (4 ,6  5 »RJ< 4  5 1  ERR INT (4,65 

DIMENSION  XU(6) ,YW<6) »ZW<6) , DRTHET < 4 ■ 6 5 

COMMON/Bl/GR,MB 

C0MM0N/B2/THETA,L 

C0MM0N/B3/M,LM, IX,IY,IZ 

C0MM0N/B4/QPX » QPY ■ QPZ • QPHI , QTHETB , QPSI > QEX , QEY , QEZ 

C0MM0N/B6/N,K 

C0MM0N/B7/TIME , R , DR , DDR 

C0MM0N/B3/AP , AR • DTHETA 7 DQ 

C0MM0N/B9/K1-K2 

COMMON/BIO/INCR 

COMMON/B11/IXO,IYO,IZO 

C0MM0N/B12/PX , PY , PZ , PHI t THETAB , PS I 

C0MM0N/B13/LP , LQ  7 LR 

C0MM0N/B14/DRTHET 

COMM0N/B15/KTHETA 

C0MM0N/B16/A 

C0MM0N/B17/LAMBDA 

COMMON/BIS/RQ 

C0MM0N/B19/DPX  > DRY , DPZ , DPHI 7  DTHETB , DPS I 

COMM0N/B20/RTHETA 

C0MM0N/B21/T 

C0MM0N/B22/ATERM 

C0HM0N/B23/XW  t  YWjZW 

C0MM0N/B24/ERRINT7LAM0LD 

C0MM0N/B25/KPX  7  KPY , KPZ , KPHI , KTHETB ?  KPS I 

C0MM0N/B26/RPX  7  RPY  7  RPZ , RPHI , RTHETB . RPSI 

C0MM0N/B27/DRPX  ,  DRPY  ,  DRPZ  7  DRPH I  1  CRTHEB  7  DRPSI 

C0MM0N/B23/DDRPX , DDRPY , DDRPZ , DDRPHI , DDRTHB  > DDRPSI 

C0MM0N/B29/DQPX  7  DOPY , DGRZ  7  DQRH I  7  DQTHEB 7  DGPSI 
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c 

C        THE  PHYSICAL  PARAMETERS  OF  THE  SYSTEM  ARE  DEFINED  HERE, 

C 

C 

GR=32.2 

PI=4,0*ATAN(1.0) 

MB-1.53 

1X0=0.250 

IY0=0,250 

IZ0=0.43£ 

DO  10  J=l,6 

L(i»J>=0.75 

L(2iJ)=0,25 

L(3r J)=1.S 

L<  4  * J><=2. 0 

M<2> J) =0,0291 

M<3,J>=0.224 

M(4.J)=0,0S3? 

LM<2>J>=0.125 

LM<3,J)=0.75 

LM(4> J)  =  l  ,0 

IX(2iJ>=0. 000211 

IY < 2, J > =0.000257 

IZC2,J>=0, 000257 

IX(3fJ)=0. 00104 

IY(3,J)=0.0425 

IZ(3,J>=0.0425 

IX(4,J!=0. 000218 

IY<4» J)=0.02S1 

IZ(4iJ)=0.0281 
10       CONTINUE 
C 

C  THE  BODY  POSITION  AND  ORIETATION  ARE  INITIALIZED 

C  IN  THIS  SECTION.   PHI,  THETAB,  AND  PS  I  ARE  PURTERBED 

C  SLIGHTLY  TO  OFFSET  SINGULARITY  PROBLEMS  IN  THE  GAU3SX 

C  SUBROUTINE. 


PX=0.0 

PY=0.0 

PZ=1.0 

PHI=1.0E-6 

THETAB=1.0E-6 

PSI=-l,0E-6 

DPX=0.0 

DPY=0.0 

DPZ=0.0 

DPHI=0.0 

DTHETB=0.0 
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DPS  I =0.0 

C 

c 

C        THE  LEG  JOINT  ANGLES  ARE  INITIALIZED  IN  THIS  SECTION. 

C 

C 

THETA<1,1)=0.0 

DO  100  J«2»(4 

THET A ( 1 , J ) =THETA ( 1 , J-l >  +PI/3 . 0 
100      CONTINUE 

DO  200  J- 1,6 

THETA(2,J>=0.0 

THETA<3,J>=-0. 601378 

THETA<4,J)=1. 73067 
200      CONTINUE 

DO  110  l"2tA 

DO  120  J=-l»6 

DTHETA<I,J>==0.0 
120      CONTINUE 
110      CONTINUE 
C 
C 

C        THE  INITIAL  SLOPES  OF  THE  SPLINE  FUNCTIONS 
C        ARE  DEFINED  HERE. 
C 
C 


DO  105  [1,4 

DO  106  J«l,6 

DRTHET(I,J)=.0.0 

106 

CONTINUE 

105 

CONTINUE 

C 

DQPX=0.0 

DQPY=0.0 

DOPZ"»0.0 

DQPHI=0.0 

DQTHEB=0.0 

HOPS I =0,0 

c 
c 

C  THE  WORKING  ARRAYS  FOR  THE  RUNGE-KUTTA-GILL 

C  SUBROUTINES  ARE  INITIALIZED  HERE.  RQ  IS  USED 

C  IN  SUBROUTINE  RILEG  AND  DO  IS  USED  IN 

C  SUBROUTINE  RIARM 

C 

c 

DO  130  1*1,12 

RQ(I)=0.0 
130      CONTINUE 

C 
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DO  140  1=1 ,6 

DO  150  J- 1.6 

DQ(I.J!=0.0 
ISO      CONTINUE 
140      CONTINUE 
C 
C 

C        THE  STORAGE  ARRAY  FOR  THE  ERROR  FUNCTION  IS 
C        INITIALIZED  IN  THIS  SECTION. 
C 
C 

DO  160  1  =  1*4 

DO  170  J  =  l,6 

ERRINTCl7j)=0.0 
170      CONTINUE 
160      CONTINUE 
C 

c 

C        THE  GAINS  AND  INERTIA'S  FOR  CONTROL  OF  THE 

C        NON  SUPPORT  LEGS  ARE  READ  IN  FROM  THE  FILE 

C        'GAIN. DAT',   THE  VARIABLE  'PROPOR'  IS  A 

C        PROPORTIONALITY  CONSTANT  USED  IN  ADJUSTING 

C        T(ZtJ)    FOR  THE  EFFECTS  OF  GRAVITY. 

C 

C 

OPEN (UNIT=10 1 FILE-' GAIN . DAT ' > STATUS* ' OLD  ' ) 

READ<10,*)IDrKM(2> 

READ(10;*)IDrKM(3) 

READ<10»*)ID>KM<4) 

READ(107*)III,RJ<2> 

REAB<iO»*>IBiRJ<3) 

REAIK10»*)ID;RJ(4) 

READ(10.*)ID;KD 

READ<10»*)ID»KP 

READ(107*)ID.KI 

READ ( 10 ,*)IDf PROPOR 

CLOSE (10) 
C 
C 

C        THE  STARTING  LOCATION  OF  THE  ENDP0INT3  OF  EACH  OF 
C        LEGS  IS  DEFINED  IN  THIS  SECTION. 
C 
C 

XU<1)=3.0 

YU<1)=0.0 

ZU(1)=0.0 

XU(2)=1.5 

YW(2)=2.598 

ZW(2)=0.0 

XU<3>=-1.5 
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YW(3)-2.59S 

2U  <  3 ) =0 , 0 
XU(4>=-3.0 
YU<4>=0.0 
ZW<4>=0,0 
XW<S)»-1.S 
YU<5>=-2.598 
ZU(5>=0.0 
XW<6)"i.S 
YU(6)=-2.5?S 
ZW<6)=0.0 
C 

c 

C        THE  DATA  POINTS  DEFINING  THE  PATH  OF  THE  BODY  AND 

C        OF  THE  END  POINTS  OF  THE  LEGS  ARE  READ  IN  FROM  THE 

C        FILE  ' INPUT < DAT 't   '■'  IS  THE  NUMBER  OF  DATA  POINTS 

C        SUPPLIED  FOR  EACH  OF  THE  BODY  AND  END  POINT 

C        VARIABLES. 

C 

C 

OPEN ( UNIT=10 t FILE  = ' INPUT . DAT ' i STATUS- ' OLD ' ) 

READ<10,*)ID 

R£ADUO»*XQPXU)»I--lfV) 

READ (10,*)  (QPY<I)  , I=1,U) 

READ(10f*XQPZ(I)  »I=1»V5 

READ (10,*)  (QPHKI)  , I=l,y • 

READ  < 1 0 , * ) ( QTHETB ( I ) » 1  =  1 » U  ) 

READ(10,*HGP3T<I>  .  1=1. V) 

DO  210  J=-1.6 

READ(1Q.*)ID 

READ*  10  ,  *  )  (  G!EX(  Jj  I)  >  I«i  ,W> 

READ (10,*) ID 

READ  ( 1 0  ,  *  >  <  QEY  (  J  .  I )  1 1  =<  1  .  V  ) 

READ<10.*)ID 

READ' 10, *) (QEZCJ, t) tl-ltV) 
210      CONTINUE 

CLOSE (10) 
C 

c 

C        THE  RESULTS  OF  THE  SIMULATION  ARE  WRITTEN  INTO 

C        THE  FILE  'OUTPUT.DAT'. 

C 

C 

OPEN  ( UNIT  =  10 1  FILE  =  '  OUTPUT  .  DAT  '  ,  STATUS" '  NEW  '  ) 
C 
C 

C        THE  SUPPORT  AND  NON  SUPPORT  LEGS  ARE  DEFINED  IN  THIS 
C        SECTION.   AP,  AC,  AND  AR,  ARE  THE  NON  SUPPORT  LEGS. 
C        LP,  LQ,  AND  LR,  ARE  THE  SUPPORT  LEGS. 
C 
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AF'=1 

AQ=3 

AR=5 

LP=2 

LQ=4 

LR=6 
C 
C 

C        THE  VARIABLES  DESCRIBING  THE  VARIOUS  TIME  PARAMETERS 
C        ARE  DEFINED  IN  THIS  SECTION.   DELTA  IS  THE  SPACING 
C        OF  THE  INPUT  DATA  ALONG  THE  TIME  AXES,   INCR  [S  THE 
C        STEP  SIZE  FOR  THE  NUMERICAL  INTEGRATION.  DEL  IS  USED 
C        IN  THE  CONTROL  OF  THE  TIMING  LOOPS  AND  THE  TRANSFER 
C        OF  BODY  WEIGHT  FROM  ONE  SET  OF  LEGS  TO  THE  OTHER. 
C        TIME  IS  A  COUNTER  USED  TO  KEEP  TRACK  OF  THE  TIME 
C        ALONG  EACH  SECTION  OF  THE  SPLINE  CURVES.   RTIME  IS 
C        A  COUNTER  USED  TO  KEEP  TRACK  OF  THE  TOTAL  SIMULATION 
C        TIME. 
C 
C 

DELTA=1.0 

INCR=0.1 

DEL=INCR/2.0 

TIME=0.0 

RTIME=0.0 
C 

c 

C        THE  SIMULATION  STARTS  HERE. 

C 

C 

C        THE  FIRST  LOOP  CONTROLS  THE  NUMBER  OF  SPLINE  SECTIONS 

C        THAT  ARE  CREATED. 

C 

C 

N-IPQINT-1 

DO  300  K-liV-N 

TIME=0.0 
C 

c 

C        SUBROUTINE  PL0Y2  CREATES  THE  NECCESSARY  SPLINES  AND 

C        THE  CORRESPONDING  POLYNOMIAL  COFFECIENTS. 

C 

C 

CALL  P0LY2 
C 
C 

C        THIS  LOOP  CONTROLS  THE  INCREMENTING  FOR  THE 
C        NUMERICAL  INTEGRATION. 
C 
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DO  400  TI"INCR»DELTA+0,5*INCRiINCR 

TIME=TI/DELTA 
RTIME-RTIME+INCR 

C 

C 

C        SUBROUTINE  SPEC2  CALCULATES  THE  VALUE  OF  THE  INPUT 

C        CURVES  FOR  THE  CURRENT  VALUE  OF  TIME.   SUBROUTINE 

C        TORQUE  CALCULATES  THE  CONTROL  TORQUES  FOR  THE 

C        SUPPORT  LEGS.   SUBROUTINE  LEG  IS  WHERE  THE  DYNAMIC 

C        SIMULATION  FOR  THE  BODY  AND  SUPPORT  LEGS  IS  DONE, 

C 

C 

CALL  SPEC2 
CALL  TORQUE 
CALL  LEG 
C 
C 

C        THE  JOINT  TORQUES  FOR  THE  NON  SUPPORT  LEGS  ARE 
C        SPECIFIED  IN  THIS  SECTION.  T(3,J>  AND  T<4»J) 
C        BOTH  CONTAIN  CORRECTIONS  TO  OFFSET  THE  EFFECTS 
C        OF  GRAVITY, 
C 
C 

DO  600  J=:AP,AR.2 
DO  630  1=2 r 4 

ERRINT(I.J)»ERRINT<ItJ)+<R(I,j)-THETA(I,J) >*INCR 
CALL  CNTRL(RJCI)  fKM  ( I )  ,  KD  i  KP  ,  KI ,  DDR  !  I ,  J  )  ,  DR  ( I  .  J  !  ,R<  I,  J) 
*  DTHETA\I,J>,THETA<I..J!  ,  ERRINT  (  I  .  J  )  ,  T  ( I  r  J  )  ) 

650      CONTINUE 

CA«LM<3tJ>*CQS(THETA<3>J) > 
CB=LM(4.J)*C0S<THETA<4,J)+THETA<3> J) ) 
CC=2.0*CA+CB 

T  <  3 1 J ! =T  i  3 • J  i -PROPOR* ( M ( 3 1 J ! *8R*CA+M < 4 • J ) *GR*CC > 
T  <  4  r J) oT (4 1 J ) -M<  4 1 J ) *GR*CB 
600      CONTINUE 
C 
C 

C        SUBROUTINE  ARM  PERFORMS  THt  DYNAMICS  FOR  THE  NON 
C        SUPPORT  LEGS. 
C 
C 

CALL  ARM 
C 
C 

C        THE  OUTPUT  FILE  IS  CREATED  IN  THIS  SECTION.  THE 
C        WRITE  COMMANDS  AND  FORMAT  STATEMENT  CAN  BE  CHANGED 
C        TO  FIT  WHATEVER  DATA  NEEDS  TO  BE  WRITTEN  TO  THE 
C        OUTPUT  FILE. 
C 


129 


c 

5       FORMAT <A20,3£15. 7) 
ID-'  THETA<2>5)' 

URITE<10jSHD>R<2i5) »THETA<2»55 .RTIME 
ID-'  TH£TA<3>5)' 

WRITE ( 10 , 5 ) ID»  R  < 3 tS ) » THETA  <3>5> . RTIME 

ID-'  THETA (4.3)' 

WRITE < 10.5 ) ID  t R<4 1$) t THETA ( 4 , 5 ) » RTIME 

ID-'  PX' 

WRITE  ',  10  1 5>IDi  RPX  i  PX  i  RTIME 

ID-'  PY' 

WRITE ( 10 ! 5 ) ID . RPY  > PY ; RTIME 

ID-'  PZ' 

WRITE <10»5> ID. RPZ.PZ, RTIME 

ID"'  PHI' 

WRITE ( 10 >S) IDjftPHI ; PHI » RTIME 

ID-'  THETAB' 

WR ITE  ( 1 0 » 5  )  I D .  RTHETB  i  THET AB  i  RT I  Mf£ 

ID-'  PSI' 

WRITE  < 10  »3 ) ID .RPSI . PSI >  RTIME 
C 
C 

C        THIS  SECTION  MONITORS  THE  RTIME  VARIABLE  SO  THAT 
C        THE  TRANSFER  OF  BODY  WEIGHT  FROM  OWE  SET  OF  LEGS 
C        TO  THE  OTHER  CAN  BE  MADE  AT  THE  DESIRED  TIMES. 
C        SUBROUTINE  XYZ  CALCULATES  THE  END  POSITIONS  OF 
C        THE  LEGS  THAT  ARE  BECOMING  THE  SUPPORT  LEGS. 
C        THE  INTEGRAL  ERROR  FUNCTION  -  ERRINT.  IS  RESET  FOR 
C        THE  JOINT  ANGLES. 
C 

c 

A=ABS(RTIME-2.0) 

B=ABS<RTIME-6.0i 

C=ABS«RTIME-10.0) 

D=ABS(RTIME-14,0) 

IF ( A . LT . DEL . OR . B , LT . DEL . OR . C . L  T . DEL . OR . D , LT . DEL ) THEN 

CALL  XYZ 

DUMBP=AP 

DUMBQ=  AQ 

DUMBR=AR 

AP=LP 

AQ=LQ 

AR=LR 

LP-DUMBP 

LQ=DUMBQ 

LR=DUMBR 

DO  800  1^2.4 

DO  850  J=l»6 

ERRINT<IrJ)-O.Q 
850  CONTINUE 
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3°0  CONTINUE 

ELSE 

CONTINUE 

END  IF 
400      CONTINUE 
300      CONTINUE 
C 
C 
C        THIS  SECTION  IS  HERE  SO  THAT  THE  PLOTTINO  PROGRAM 

CAN  DETECT  THE  END  OF  THE  OUTPUT  FILE. 

C 

ID» 'END' 

DO  900  1=1,9 

WRITE < 1 0  »5> ID » 1 . 0 1 2 . 0 » 3 . 0 

900      CONTINUE 

C 

C 

c: 

CLOSE(IO) 

STOP 
END 

c 

C        SUBROUTINE  TORQUE  PRODUCES  THE  JOINT  TORQUES  FOR 
C        THE  SUPPORT  LEGS. 


SUBROUTINE  TORQUE 

DOUBLE  PRECISION  LAM ( 6 > , C ( 6 i 7 ) 

D I MEMS I ON  T  <  4 » 6 > ,  ERR  I  NT ( 4f  6  > , A  <  9 1 15 ) , QT ( 6) 

REAL  I NCR , KM >  KD » KP i KI » J t IXO . I YO 1 1 ZO 

REAL  LAfil,LAM2,LAM3,LAM4,LAM5,LAM6,LAM7,L  AM8,l  Ai19 
INTEGER  P.G.fi  "" 

CHARACTER*80  ID 

C0MM0N/B10/INCR 

COMMON/BH/IX0»IYO»IZ0 

COMMON/B 1 2/PX , PY « PZ  p PH I , THETAB » PS  I 

C0MM0N/B13/P»Q,R 

C0MM0M/B16/A 

C0MM0N/B19/DPX , DPY , DPZ , DPHI , DTHETB , DPSI 

C0MM0N/B21/T 

C0MM0N/B24/ERRINT,LAM0LD 

C0MM0N/B26/RPX , RPY , RPZ , RPHI , RTHETB , RPSI 

C0MM0N/B27/DRPX , DRPY , DRPZ , DRPHI , DRTHEB , DRPS I 

C0MM0N/B28/BDRPX .  DDRPY  ,  DDRPZ ,  DDKRHI ,  IiDRTHB ,  DDRPS I 


C        ^EQ  IS  THE  NUMBER  OF  UARIABLES  SOLVED  EOF 


'<  IN  THE 
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GAUSSX  SUBROUTINE.   J  IS  THE:  MASS  OF  THE  BODY, 


NEG=6 

NEQ1=NEQ+1 

J=1.55 
C 
C 

C  THE  GAIN  VALUES  FOR  THE  BODY  CONTROL  SYSTEM  ARE 
C  READ  IN  FROM  THE  FILE  'BODY. DAT' .  ALSO  READ  IN 
C  IS  THE  VARIABLE  DEL  WHICH  IS  USED  IN  THE  BAUSSX 
C  SUBROUTINE  WHEN  THE  BODY  ORIENTATION  ANGLES  GET 
C  CLOSE  TO  ZERO. 
C 
C 

OPEN  (  UNIT-1 1 .  FILE=  '  BODY  ,  DAT  '  «  STATUS-'  OLD  '  * 

READ<llf*)ID»KM 

READ<11>*>ID>KD 

READU1.*)IDjKP 

REAIi(11,*5ID,KI 

READ<11»*>ID*DEL 

CLOSE(li) 
C 
C 

C        SUBROUTINE  ACOEF  CALCULATES  ALL  OF  THE  PARTIAL 
C        DERIVATIVES  OF  THE  CONSTRAINT  EQUATIONS. 
C 
C 

CALL  ACOEF 
C 
C 

C        THE  GENERALIZED  BODY  CONTROL  FORCES  ARE  DEVEI  OPED 
C        IN  THIS  SECTION.   ERRINT  IS  THE  INTEGRAL  OF  THE 
C        ERROR  FUNCTION.   T3  IS  MODIFIED  TO  COUNTERACT  THE 
C        GRAVITATIONAL  FORCE  ON  THE  BODY.   NOTES   THIS 
C        CORRECTION  FACTOR  ON  T3  IS  ONLY  VALID  WHEN  THE  BOO',' 
C        IS  IN  A  NEAR  LEVEL  POSITION. 
C 
C 

ERRINT  ( 1  >  1  )  =ERR  INT  (  1  , 1  >  +  <  RPX-PX  )  *  I  NCR 

CALL  CNTRL  (  J  >  KM  7  KB ;  KP  ,  KI ,  DDRPX  ,  DRPX  .  RPX  .  DPX  ,  PX  . 

*  ERRINT ( 1, 1 5 ,T1) 

ERRINT (1,2) =ERRINT  <1  ,  2 !  +  <  RPY-PY ) * INCR 

CALL  CNTRL  (  J  .  KM ,  KD ,  KP  ,  K I ,  DDRPY  -  ORPY  ,  Pp  '■'  -  BPY  -  PY  . 

*  ERRINT<1,2)»T2) 

ERRINT  ( 1 ,  3  )  =ERRINT  < 1  .  3  )  +  <  RPZ-PZ  )  *  I  NCR 
CALL  CNTRL  (  J  .  KM ,  KD  ■>  KP  r  KI  r  DDRPZ  >  DRPZ  .  RPZ  ,  DPZ  ,91 , 
%  ERRINT(1,3) »T3) 

T3=T3+4?.?1 
ERRINT ( 1 , 4 ) =ERRINT ( 1  -  4 !  +  ( RPH I-PH I ! * INCR 
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J=IZO*COS <  THETAB ) +IYOHSIN < THETAB ) 

KM* J/2. 5 

CALL  CNTRL ( J t KM > KD » KP > K I > DDRPH I . DRPH I , RPH I . 

DF'HI ,  PHI , ERRINT  (  I ,  -1 )  ,  T  1 ) 
ERRINT ( 1 . 5 ) =ERRINT ( 1 1 5 )  +  •  RTHETB-THETAB  >  *INCR 
J-0 ♦ 25 
KH=0.1 
CALL  CNTRL  < J i KM • KD i KP , KI i DDRTHB , DRTHEB  » RTHETB i 

DTHETB i THETAB  t ERR I NT U  >5)  j T5 ) 
ERRINT  (1.6)  =ERRINT  i  1  >6)  +  <  RPSI-PSI )  *I*4CR 
J=0.436 
KM=0.1744 
CALL  CNTRL ( J , KM i KD , KP . KI , DDRPSI t DRPSI > RPSI > 

DPSI  •  PSI  i  ERRINT  ( 1  ,  6)  »T<4? 


CCMtN)  IS  THE  COEFFICIENT  MATRIX  USED  BY  THE  GAU3SX 
SUBROUTINE  TO  SOLVE  FOR  THE  SET  OF  LAMBDA'S  WHICH 
RESULT  FROM  THE  GENERALIZED  BODY  FORCES  Tl  THRU  T6. 
LAM(M>  IS  A  WORKING  SUBROUTINE  USED  TO  TRANSFER  THE 
LAMBDA'S  FROM  THE  GAUSSX  SUBROUTINE  TO  THE  TOROUE 
SUBROUTINE.  LAM1THRU  LAH9  ARE  THE  RESULTING  LAMBDA'S 
USED  TO  CALCULATE  THE  SUPPORT  LEG  JOINT  TORQUES. 


CU 

cu 

C(i 
CU 

cu 

CU 

cu 

C<2 

C(2 
C(2 
C(2 
C(2 
C(2 
C(2 
C(3 
C(3 
C<3 
C(3 
C(3 
C(3 
CC3 
C(4 
C<4 
C(4 
C(4 


i>' 


+AC 
+  A(< 


'DBLE<AU»i>+A< 

:DBLE(AUU)+A< 
:DBLE(0.0> 
iDBLE(O.O) 
■DBLE(O.O) 
'DBLE(O.O) 
DBLE(Tl) 
TiBLE(O.O) 
TiBLECO.OJ 
'DBLE<A<4 
DBLE<A<5 
■DBLE'.Q.O) 
TiBLE(O.O) 
TiBLE(T2) 
•DBL£<0.0) 
'DBLE(O.O) 
>BBLE<0.0) 
•DBLE(O.O) 
DBLE<A(7f3)+A< 
DBLE(A(8f3)TA< 
DBLECT3) 
DBLE(AU>4)+A< 
DBLE<AUj4)+A< 
DBLE(A(4?4J+A( 
DBLE<A<5f4)+A< 


2u; 
3(  i: 
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C(4,5)=DBLE(0.0) 
C<4t6!=DBLE(0.0) 
C<4»7)»DBLE(T4) 
C  (  5  >  1  > =DBLE  (  A ( 1 . 5 ) +A  < 2 , 3 ) ) 
C  ( 5  >  2 ) =DBLE CA< 1 > 3)+ft (3 »5>  > 
C<5»3 )»DBLE( A  <  4  »5) +A( 5»5  > ) 
C<5»4)»DBi.E<A<5»5>+A<6>5!  ) 
C(5,3)=DBLE(A(7,5)+A(9,5>  ) 
C  <  5 » 6 ) =DBLE ( A ( 8  ,  5 ! +A ( 9  > 5 ) ) 
C(5,7)=DBLE(T5) 
C  ( 6  , 1 )  =DBLE  <  A  ( 1 ,  6  )  +  A  (  2  ,  6  )  ) 
C ( 6 , 2 ) =  DBLE ( A ( 1 1 6 )+A<3 > 6  >  ) 
C(6»3)=DBLE(A<4»6)+A(5,6)  ) 
C  (  6 , 4 ) =DBLE ( A ( 5  > 6 ) +A  < 6 , 6 ) ) 
C(6,5)=DBLE(A(7,6)+A(9,6>  ) 
C  ( 6 , 6 ) =DBLE ( A<8 1 & >+A  <  9  r 6 ) ) 
C<6f?)=DBLE<T6) 

CALL  GAUSSX  (CLAM.  DEL  »NEG,NEQ1) 
LAM2=LAMC1) 
LAM3=LAM(2) 
LAM4=LAM(3) 
LAM6=LAM(4) 
LAM7=LAM(5) 
LAM8=LAM(6> 
LAM1=LAM2+LAM3 
LAM5=LAM4+LAM<4 
LAM9=LAM7+LAM8 
C 

c 

C        THE  SUPPORT  LEG  JOINT  TORQUES  ARE  COMPUTED  IN  THIS 
C        SECTION. 

C 

c 

T(2>P)»-(A(i,7) *LAH1+A (4,7) *LAM4+A (7,7) *LAH7  > 
T ( 3 , P  >  =- <  A ( 1  ,  3  )  *LAM1+A  (4,8  )  *LAM4+ A (  7,8)  *LAM7  ) 
T ( 4 , p ) =- ( A ( 1 , 9 ) *L AM 1 +A ( 4  ,  9 ) *LAM4+A (7,9) *LAM7 ) 
T ( 2 , Q ) =- ( A  <  2 , 10  >  *LAM2+A (5,10) *LAM5+A (3,10) *LAM8 ) 
T ( 3 , Q  >  =- ( A ( 2 , 1 1 ) *LAM2+A (5,11) *LAM5+ A ( 8.11) *LAK8 ) 
T  (  4  ,  Q )  =-  (  A  (  2  , 1 2.)  *LAM2+A  (5,12)  *L AM5+A  (8,12)  *LAM8  ) 
T  (  2 ,  R  )  =-  (  A  (  3 , 1 3 )  *LAM3+A  (6,13)  *LAM6+ A  (  9  ,  1. 3 )  *L  AM9  ) 
T  ( 3  ,  R  )  =-  (  A  ( 3  , 1 4  )  *LAM3+ A  (  6  , 1 4  )  *L AM6+  A  (  9,14)  KL.AM9  ) 
T ( 4 , R ) =- ( A ( 3 , 1 5 ) *LAM3+A ( 6 , 1 5 ) *L AM6+ A (9,15)*!  AM9 ) 
RETURN 
END 

C  *  *  *  *  *  ***************  ;|:  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  :i:  i  *  *  *  *  %  %  %  %  *  %  %  *  %  %%%  x  %  *  %  % 

C 

c 

C        SUBROUTINE  OAUSSX  IS  A  GAUSSIAN  ELIMINATION 

C        SUBROUTINE  USED  TO  SOLVE  THE  COEFFICIENT  MATRIX 

C        C(M,N)  IN  THE  TORQUE  SUBROUTINE. 
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c 

c 

SUBROUTINE  GAUSSX ( A , X »DEL . N i Nl > 

DOUBLE  PRECISION  A  (  N  :  Nl )  .  X  (  N  S  r  BIG  »  DUMMY  »  S  7  AU 

DO  200  J~1>N 

J1=J+1 

IF<J1,6T.N)  60T0  930 

BIG  =  ABS<A(JI J)  ) 

M=J 

DO  900  L"J1»N 

IF<ABS(A<L7.J)  >  .LE.BIGJ  GOTO  900 

M=L 

BIG=ABS<A(L>J;  ) 
900      CONTINUE 

DO  990  JJ=J,N1 

DUMMY=A<M>JJi 

ACM»JJ>«"A<Jr-JJ) 
990      A(J>JJ)=DUMMY 
980      CONTINUE 
C 

C        DEL  IS  THE  SMALLEST  VALUE  THAT  A < J , J )  CAN  TAKE  ON, 
C       THIS  IS  BECAUSE  THE  Cifi.N!  MATRIX  BECOMES  POORLY 
C        DEFINED  AT  EXTREMELY  SMALL  BODY  ORIENTATION  ANGLES. 
C 

IF<ABS<A(J>J>}  .LT.DBLE(DEL)  )  A <  Jj  J)*DBLECDEL  ) 
C 
C 

S=1,0/A(J?J> 

DO  201  K=;J.N1 

201  A(J»K)=A<JrK)*S 
DO  202  1=1  >N 
IF(I-J)  203»202>203 

203  AIJ=-A(l7j) 
DO  204  K=J»N1 

204  A ( I , K ) =A ( I • K } + A I J*A ( J . K ) 

202  CONTINUE 
200      CONTINUE 

DO  300  J>1>N 
300      X(D=A(I»N1j 
RETURN 
END 

C 

c 

C  SUBROUTINE  CNTRL  RETURNS  THE  CORRECTIVE  FORCE  UHEN 

C  GAINS;  INERTIA'S?  AND  INPUT  SIGNAL'S  SUPPLIED  !'C  IT, 

C  SUBROUTINE  CNTRL  IS  USED  BY  THE  NON  SUPPORT  LEGS  AS 

C  WELL  AS  THE  BODY. 

C 

C 
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SUBROUT I NE  CNTRL  '  J  t  KM ,  KD  » KP>  KI ,  DDR.  DR  ,  R  ,  DC  ,  C  -  ERR  I  NT  i  T  ) 

REAL  JtKM»KD»KP»KI 

T=J*DDR+KM*KDS:  ( DR-BC)  +KM*KP*  ( R-C  >+KM*KI*ERRINT 

RETURN 

END 

c 
c 

C        SUBROUTINE  P0LY2  OVERSEES  THE  CREATION  OF  ALL  THE 

C        NECCES3ARY  SPLINE  FUNCTIONS.   KPX»  KPY,  KPZ<  KPHI » 

C        KTHETBi  KP8I.  AND  KTHETA  STORE  THE  POLYNOMIAL 

C        COEFFICIENTS  OF  THE  SPLINES.   RTHETA  STORES  THE 

C        DISCRETE  JOINT  ANGLES  COMPUTED  FROM  THE  INPUT  DA  1 A 

C        OF  THE  BODY  AND  LEG  END  POSITIONS. 

C 

C 

SUBROUTINE  P0LY2 

PARAMETER  (  U=17  ,  IPOINT  =  ,5  > 

DIMENSION  RTHETA  C4 1& 1 1  POINT ) , QPOLY ( 0 ! IPOINT-1 ) 

DIMENSION  DRTHET(4r6> 

DIMENSION  QPX(U)  »QPY<V5  iQPZCV)  jOPHKV)  ,  BTHETB  ( V  > 

DIMENSION  QPSI  ( V  )  ,QEX  1 6 >  V  )  »  QE Y  (  6 r  V )  . SEZ  (  6  t  V  > 

REAL  KPX ( 0 ! 3 ) ?  KPY  <  0 : 3 ; , KPZ (013)?  KPHI ( 0 : 3 > , KTHETB ( 0 ! 3 ) 

REAL  KPSI ( 0 J3>  r KPOLY (013)? KTHETA ( 4 »6» 0 ! 3 > 

C0MM0N/B4/QPX i QPY , QPZ . QPHI . QTHETB • QPSI » HEX , Qr Y  » QEZ 

C0MMON/B6/NtK 

C0MM0N/B?/KlrK2 

C0MM0N/B14/DRTHET 

C0MM0N/B15/KTHETA 

C0MM0N/B20/RTHETA 

C0MMQN/B25/KPX , KPY . KPZ . KPH I , KTHETB , KPSI 

C0MM0N/B29/D0PX  t DQPY  > DQPZ , DQPH I r DOTHEB  ? DOPSI 
C 
C 

C        THE  DISCRETE  JOINT  ANGLES  ARE  COMPUTED  FROM  THE 
C        BODY  AND  LEG  END  POINT  DATA  IN  THIS  SECTION, 
C 
C 

IF«K,Ea,l)THEN 

DO    100    Kl~  LI  POINT 
K2=K1 

CALL  INUERS 
100  CONTINUE 

ELSE 

DO  200  N1-1>N 
DO  300  J--l?6 
DO  400  1=2 i 4 

RTHETA  ( I ,.  J  ,  Ml )  =RTHETA  < I . J.N1+1 ) 
400  CONTINUE 

300  CONTINUE 
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200  CONTINUE 

K1=K+N 

K2=IP0INT 

CALL  INVERS 
END  IF 
C 
C 

C        THE  INDIVIDUAL  JOINT  SPLINE  FUNCTIONS  ARE  COMPUTED 
C        IN  THIS  SECTION.   DRTHET  IS  THE  SLOPE  AT  THE  BI-IGGINING 
C        OF  THE  CURRENT  SPLINE  INTERVAL.   9P0LY  AND  OGPOLY 
C        ARE  WORKING  VARIABLES  USED  TO  PASS  PARAMETERS  TO 
C        THE  SPLINE  SUBROUTINE. 


DO  500  I»2f4 

DO  600  J>lr6 

DO  700  M=l>IPOINT 

QPOLY (M-l ) =RTHETA ( I , J  - M ) 
700      CONTINUE 

DQPOLY=DRTHET(IiJ> 

CALL  SPLINE ( OPOLY  t KPOLY. DQPOLY ) 

DRTHET<I»J)«DQPQLY 

DO  300  M=0f3 

KTHETA ( I > J > M ) -KPGLY ( Hi 
800      CONTINUE 
600      CONTINUE 
500      CONTINUE 
C 

c 

C        THE  SPLINE  FUNCTIONS  FOR  THE  BODY  POSITION  AND 

C        ORIENTATION  ARE  COMPUTED  IN  THIS  SECTION,  DGPX 

C        THRU  DQPSI  ARE  THE  SLOPES  AT  THE  8EQ6INING  OF 

C        THE  CURRENT  SPLINE  SECTION. 

C 

C 

CALL  BSPLIN<QPX>KPXjIiGPX) 

CALL  GSPLIN ( QPY ,KPY r DOPY ) 

CALL  GSPLIN ( BPZ . KPZ i DQP2  ) 

CALL  GSPLIN < GPHI • KPHI , DGPHI } 

CALL  GSPLIN ( QTHETB . KTHETB • DQTHEB ! 

CALL  GSPLIN<QPSI»KPSI>IIGPSI) 

RETURN 

END 

c 
c 

C  SUBROUTINE  GSPLIN  IS  AN  INTERMEDIATE  SUBROUTINE 

C  FOR  THE  COMPUTATION  OF  THE  BODY  SPLINES.   THE 

C  BODY  VARIABLES  ARE  PUT  INTO  WORKING  VARIABLES 

C  WHICH  ARE  THEN  PASSED  TO  THE  SPLINE  SUBROUTINE, 
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c 
c 

SUBRQUT I  ME  GSPLIN ( QBODY t KBODY  t OQBODY ) 
PARAMETER  <V=17,IP0INT'=3) 
REAL  KB0DY(0I3) 

DIMENSION  QBODY  <U> » SPOLY (0 JIPOINT-1 ) 
C0MM0N/B6/N>K 
DO  100  M=0,N 
QPOLY<M)=QBODYCK+M) 
100      CONTINUE 

CALL  SPLINE ( QPOLY , KBODY , DGBODY ) 

RETURN 

END 

C 

C 

C        THE  SPLINE  CURVES  ARE  COMPUTED  IN  SUBROUTINE  SPLINE. 

C        Q  IS  THE  DATA  SET  USD  TO  CREATE  THE  SPLINE.   KP  IS 

C        THE  POLYNOMIAL  COEFFICIENTS  THAT  DESCRIBE  THE  SPLINE. 

C        DO  IS  THE  SLOPE  AT  THE  BEGGINING  OF  THE  SPLINE  CURVE 

C        FOR  THE  CURRENT  SECTION. 

C 

c 

SUBROUTINE  SPLINE< Q ,KP»DQ> 

PARAMETER  <IP0INT=3> 

REAL  KP(0:3) 

DIMENSION  CKO:iPOINT-l),P<IPOINT-l ! ,R< [POINT) 

DIMENSION  S(IPOINT)  ,T<  IPOINT)  ,A< -1 12) 
C 
C 

C        IF  LESS  THAN  4  DATA  POINTS  ARE  USED  TO  COMPUTE  THE 
C        SPLINE  CURVES,  THE  RECURSIVE  CONSTANTS  ARE  NOT  NEEDED. 
C 
C 

N=IP0INT-1 

IFCN.LT.3!  GOTO  200 
C 

c 

C        THE  RECURSIVE  CONSTANTS  USED  TO  COMPUTE  Ail)  ARE 

C        CALCULATED  IN  THIS  SECTION. 

C 

C 

P ( 1 ) =6 . 0 

P(2)=-24.0 

DO  300  1-3, N 

P  ( I  )  =-4  .  0*P  '.  I  - 1  )  -p  <  I  -2  > 
300      CONTINUE 

R(2)=-1.5 

R<3)=6.0 

DO  400  1=4, N+l 
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R<I)«-4.0*R<I-i>-R(I-2) 

400  CONTINUE 

S { 2 ) =-0  4  5 

S(3)=2,0 

DO    500    IM,N+1 

S(I>=-4.0*SCI-l>-S(I-2) 
500      CONTINUE 

T<2>=-3.5 

T<3)=13.0 

DO  600  I*4»N+1 

T<I)=-4.0*T<I-l>-T<I-2> 
600      CONTINUE 
C 
C 

C        A(l)  IS  COMPUTED  IN  THIS  SECTION  U1HEN  4  OR  MORE 
C        DATA  POINTS  ARE  USED  IN  CREATING  THE  SPLINE. 
C 
C 

Z=0.0 

DO  700  1=1, N 

Z=Z+P<I)*Q(N+1-I> 
700      CONTINUE 

Y-=0.0 

DO  800  1=1, N-2 

Y=Y+P(I)*Q<N-1-I) 
300      CONTINUE 

A  ( 1 )  =  (  Z- Y+ ( R ( N+ 1 ) -R  t N- 1 > ) *Q < 0 )  +  < S ( N+ 1 ) -S  !  N-  1  i  )  X 
*   DQ)/(TiN-l)-T(N+l) ) 

GOTO  900 
C 

c 

200  IF(N.EQ.l)  GOTO  1000 

C 

C  THIS  EQUATION  IS  USED  UHtN  3  DATA  POINTS  ARE  USED 

C  TO  CREATE  THE  SPLINES. 

C 

A  < 1 ) =-1 . 0/6 . 0*110-0 . 5*G ( 0 ) +2 . 0*0 (1 > -0 . 5*0  < 2 ) 

SOTO  900 
C 

C  THIS  EQUATION  IS  USED  WHEN  2  DATA  POINTS  ARE  USED 

C  TO  CREATE  THE  SPLINES. 
C 

1000  A( 1)=- 1.0/3. 0*DQ-Q<0>+2. 0*0(1) 
C 

c 

C        A(0),  Ai-1>,  AND  A(2)  ARE  CALCULATED  IN  THIS  SECTIOh 
C 
C 

900      A  <  0 )  =  1 . 5*0 ( 0 ! +0 . 5*DQ-0 . 5*A ( 1 ) 
A«-l)=Af 1 >-2.0*D0 
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AC 


c 
c 
c 
c 
c 
c 
c 
c 


-■6  .  0*0  ( 1  )-4. 0*A  ( 1  )-A<  0 ) 


THE  POLYNOMIAL  COEFFICIENTS  THAT 
ARE  CALCULATED  IN  THIS  SECTION. 


DEFINE  THE  SPLINE 


KP  <  3  >  =  ( -A ( -1 ) +3 . 0*A ( 0 ) -3 . 0*A ( 1 >+A<2 ) ) fh . 0 
KP  ( 2 )  =  ( 3 . 0*A ( - 1 ) -6 . 0*A ( 0 ) +3 . 0*A  < 1 ) ) /6 , 0 
KP ( 1 )  =  < -3 . 0*A  < - 1 ) +3 . 0* A ( 1 ) ) /6 . 0 
KP ( 0 )  =  <  A ( - 1 ) +4 . 0*A ( 0 ) +A  < 1 ) ) /6 . 0 


DQ  IS  SET  EQUAL  TO  THE  END  SLOPE  OF  THIS  SECTION. 
WHEN  THE  SPLINES  FOR  THE  NEXT  SECTION  ARE  COMPUTED, 
DQ  WILL  THEN  BE  USED  AS  THE  BEGGINING  SLOPE.  THUS 
ASSURING  CONTINUITY  OF  THE  FIRST  DERIVATIVE. 


DQ=0  .  5* A  ( 2  )  -0  .  5*A  (  0  ) 

RETURN 

END 
C****************:i;*5(:*s:*******************:x*********;K;l!***:l!*:K* 
C 
C 

C        SUBROUTINE  INVERS  COMPUTES  THE  JOINT  ( 
C        THE  INPUT  BODY  AND  LEG  END  POINT  DATA. 
C 
C 


MGLES  THAT  FI1 


SUBROUTINE  INVERS 
PARAMETER  <V»17f IP0INT=3> 
DIMENSION  QPXiV) >QPY<V) »QPZ< V) 
DIMENSION  GPSKV) ,QEX(6.V) »QEY 
DIMENSION  GEZi.i-M)  ,RTHETA<  4>6t 
REAL  L<4t6) 
C0MMON/B2/THETA.L 
C0MM0N/B4/QPX>QPY 
C0MM0N/B?/KlrK2 
C0MM0N/B20/RTHETA 


QPHICV>  »OTHETB(V) 


IPOINT) i THETAf  4, 


QPZ  ,  QPHI , QTHETB . QPSI , QEX , QEY , QEZ 


SOME  COMMON  TERMS  ARE  COMPUTED  IN  THIS  SECTION. 


cphi=c0s(qphkk1)  ) 
sphi=sin<qphI':ki)  > 
cthetb=c0s  <  qthetb ( k 1 >  > 
sthetb=s in (qthetb <k1> ) 
cpsi=c0s(qpsi(k1) ) 
spsi==sin(qpsi<k1)  ) 
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TERM01=CPHI*CPSI-SPHI*CTHETB«SPSI 

TERM02=SPHI*CPSI+CPHI*CTHETB*SP3I 

TERM03=STHETB*SPSI 

TERM04=-CPHI*SFSI-SPHI*CTHETB*CPSI 

TERM05=-SPHI*SPSI+CPHI*CTHETB*CPSI 

TERM06=STHETB*CPSI 

TERM07=SPHI*STHETB 

TERM08=-CPHI*3THETB 
C 
C 

C        THIS  SECTION  CALCULATES  THE  END  POSITION  OF  THE 
C        LEGS  IN  THE  BODY  (XOiYO.ZO)  COORDINATE  SYSTEM. 
C 
C 

DO  100  J»l,6 

X=  (  QEX  (  J  t  Kl  >  -GPX  ( Kl )  )  -CTERM01  +  (  GEY  <  J  .  K 1 )  -QPY  C  Kl  >  ) 

*  *TERM02+<QEZ< J* Kl ) -GPZ < Kl > >*TERn03 

Y=  C  QEX ( J , K 1 ) -QPX ( K 1 ) ) JSTERM04  +  ( QEY  t  Jt Kl ) -QPY ( K 1 > ) 

*  *TERM05+<QEZ<JrKl)-GPZ<Kl) >*TERM06 

Z=  <  QEX  C  J , K 1 > -QPX <Ki ) >  *TERM07+< QEY ( J , K 1 ) -QPY ( K  I )  ? 
»     *TERh08+(QEZ(JrKli-GPZ(Kl) )*CTHETB 
C 

c 

C        THE  JOINT  ANGLES  THAT  FIT  THE  BODY  AND  LEG  END 

C        POSITION  ARE  COMPUTED  HERE. 

C 

C 

C1=C0S(THETA<1»J> ) 

S1=SINCTHETA(1tJ>> 

RTHETA(2tJiK2)=ATAN< ( C1*Y-S1*X ) / (C1*X+S1*Y-L ( 1 . J > ) ) 

C2=C0S(RTHETA(2.JrK2)) 

C12  =  C0S < THETA (It  J) +RTHETA ( 2  r  J  r K2 ) ) 

312=SIN<THETA<1.J)+RTHETA<2»J»K2> > 

C4=( < (X-LC1»J)*C1-U<2»J)*C12)/C12)**2+Z**2-L(4»J>4« 
9  -L  ( 3  i  J  )  **2  )  /  ( 2  .  0*L  <  3  .  J  )  *L  (  4  >  J  )  ) 

S4=SQRT<1.0-C4**2> 

RTHETA ( 4 . J  7  K2 ) =ATAN2 <  S4 , C4  > 

RTHET A ( 3 , J  r  K2 ) = AT AN  i ( L ( 4 , J ) *S4*  <  C 12*X  +  S 1 2* Y-L  U >  J  > *C 

*  -L(2»J1 )+Z*(L(4;J)*C4+L(3jJ>) ) 

*  /  >  (  L  (  4 .  J )  *C4  +  L  ( 3  .  J  )  )  :ic  <  L  ( 1  •  J  ) 

*  *C2  +  L  <  2 . J )-C12*X-S12*Y  > +Z*L (4 1 J ) tSA  )  ) 
100      CONTINUE 

RETURN 

END 
C*************************S**********************  ********** 
C 

c 

C  SUBROUTINE  SPEC2  COMPUTES  THE  VALUE.  FIRST,  AND 
C  SECOND  DERIVATIVE  OF  THE  SPLINE  CURVES  THAT  ARE 
C        USED  TO  GUIDE  THE  WALKING  ROBOT. 
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c 
c 

SUBROUTINE  SF'EC2 

DIMENSION  R<4»63 <DR(4i6> ,DDRC4,6> 

REAL  TIME  i  KTHET A  (  4  , 6  •  0  I  3 )  ,  KPX  <0  S3 )  r KPY < 0 53) 

REAL  KPZ(0:3)  .KPHK0S3)  » KTHETB < 0 5 3 >  »KPSX<0:3) 

C0MM0N/B7/TIME , R , DR , BUR 

C0MM0N/B15/KTHETA 

C0MMON/B25/KPX  >  KPY  ,  KP2  ,  KPHI ,  KTHETB  ,  KPS I 

C0MM0N/B26/RPX , RPY  , RPZ  , RPHI >  RTHETB •  RPS I 

C0MM0N/B27/HRPX , DRPY , DRPZ >  DRPH I . ORTHEB  »  DRPS I 

C0MM0N/B28/DDRPX  .•  DDRPY  .  DDRPZ  ,  DDRPHI  ,  ODRTHB  >  DDRP9I 

T2=TIME**2 

T3=TIME**3 

C 

C 

C        THIS  SECTION  COMPUTES  THE  VALUES  AMD  DERIVATIVES 

C        FOR  THE  BODY. 

C 

C 

RPX=KPX ( 3 ) *T3+KPX ( 2 ) *T2+KPX ( 1 ) *TIME+KPX ( 0  ! 
DRPX=3  . 0*KPX  (  3 >  *T2  +  2 .  0*KPX  ( 2 )  S:TI ME  +  KPX  C 1 } 
DDRPX=6 . 0*KPX ! 3 ) *TI ME  +  2 . 0*KPX ( 2  5 
RPY=KPY ( 3 ) *T3+KPY < 2 ) *T2+KPY ( 1 ) *T IME+KPY ( 0 ) 
DRPY=3 . 0*KPY ( 3 ) *T2  +  2 . 0*KPY ( 2 ) *TIME+KPY ( 1 ) 
DDRPY=4 , 0*KP  Y  (  3 ) *T I ME  +  2 , 0*KPY  t 2 ) 
RPZ=KPZ  <  3  > *T3+KPZ ( 2 ) *T2+KPZ ( 1 ) *TI  ME  +  KPZ ( 0 ) 
DRPZ=3  .  0*KPZ  (  3 i  *T2+2 .  0*KPZ  (  2  )  *TIME  +  KF'Z  <  1  ) 
DDRPZ=6 .  0*KPZ  ( 3  )  >KTIME  +  2  .  0*KPZ  <  2  ) 

RPHI=KPHI<3  )*T3  +  KPHI(2)*T2+KPHI(1)*TIME  +  KPHK0) 
DRPHI»3.0*KPHI<3>*T2+2,0*KPHIC2)*TIME+KPHI<1) 
DDRPHI»6.0*KPHI(3)*TIME+2.0*KPHI(2> 
RTHETB=KTHETB  (  3  )  *T3+KTHETB  (  2  )  *T2+KTHETB  <  1.  I  .'KTIME 
*  +KTHETB(0> 

DRTHEB=3 . 0*KTHETB ( 3 ) *T2  +  2 . 0*KTHETB ( 2 ) *T I ME+KTHETB  < 
DDRTHB=6 . 0*KTHETB ( 3 ! *TI ME+2 . 0*KTHETB  < 2 ) 
RPSI=KPSI ( 3 ) *T3+KPSI ( 2 ) *T2+KPSI ( 1 ) *TIME+KPS I ( 0 ) 
DRPS  1  =  3  .  OUtKPSI  ( 3  )  *T2  +  2  .  0*KPSI  (  2 )  *TIME  +  KPSI  ( 1  > 
DDRPSI  =  6.0*KPSK3)*TIME  +  2,0:):KPSI(2:' 

C 

C 

C        THE  VALUES  AND  DERIVATIVES  OF  THE  JOINT  SPLINES 

C        ARE  COMPUTED  IN  THIS  SECTION.   ALTHOUGH  SPLINES 

C        ARE  CALCULATED  FOR  THE  SUPPORT  LEGS  AS  WELL  A3 

C        THE  NON  SUPPORT  LEGS,  ONLY  THE  NON  SUPPORT  I  EGS 

C        ARE  GUIDED  BY  THESE  SPLINES. 

C 

C 

DO  100  J=l,6 
DO  200  1=2,4 
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R  <  I  >  J  )  =KTHETA  <  I ,  J  ,  3  )  Y.T3  +  KTHETA  (  I ,  J  ,  2  )  :S:T2  +  KTHET  A  (  I  .<  J  ,  1 
*         *TIME+KTHETA(I»JfO) 

DR ( I » J ) =3 . 0*KTHET A ( I r J  r 3  >*T2+2 . 0*KTH£TA ( I • J » 2  J *T I ME+ 
$  KTHETACI»J»1> 

DDR ( I t  J ) =6 . OKKTHET A  < I ,  J  .  3  > *TIME+2 . 0*KTHETA ( I • J , 2 ) 
200      CONTINUE 
100      CONTINUE 
RETURN 
END 

c 
c 

C        SUBROUTINE  LEG  CONTROLS  THE  DYNAMIC  COMPUTATION 

C        FOR  THE  BODY  AND  SUPPORT  LEGS, 

C 

C 

SUBROUTINE  LEG 

DIMENSION  AC9.15) 7T<4>6> .RQQ2) ,ATERM<27> 

DIMENSION  Y<12>>DYU2> 

REAL  LAMBDA(9) ,  INCR 

INTEGER  P.Q.R 

COMMON/B10/INCR 

C0MM0N/B12/PX,PY>PZ>PHI,THETAB>PSI 

C0MM0N/B13/P.Q»R 

C0MM0N/B16/A 

C0MM0N/B17/LAMBDA 

C0MM0N/B1S/RQ 

C0MM0N/B1 9/DPX , DRY , DPZ , DPH I ; DTHETB • DPS  I 

C0MM0N/B21/T 

C0MMON/B22/ATERM 
C 
C 

C        NEQ  IS  THE  NUMBER  OF  FIRST  ORDER  EQUATIONS  IN  THE 
C        RUNGE-KUTTA-GILL  SUBROUTINE, 
C 
C 

NEQ=12 
C 
C 

C        SUBROUTINE  ACOEF  CALCULATES  THE  PARTIAL  DERIVATIVES 
C       OF  THE  CONSTRAINT  EQUATIONS,   SUBROUTINE  ATERMS 
C        CALCULATES  SOME  COMMON  TERMS  THAT  ARE  MADE  UP  OF  THE 
C        PARTIALS  OF  THE  CONSTRAINT  EQUATIONS, 
C 
C 

CALL  ACOEF 

CALL  ATERMS 
C 
C 
C        THE  GENERALIZED  JOINT  FORCES  ARE  SET  EQUAL  TO  THE 
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C        SUPPORT  LEG  JOINT  TORQUES  IN  THIS  SECTION. 

C 

C 

T7=T<2,P> 

T3=T<3-P> 

T9=T(4,P) 

T10=T<2>Q> 

TU=T(3»Q) 

T12=T<4»Q> 

T13=T(2»R) 

T14=T<3>R> 

T1S»T(4»R) 
C 

c 

C        THE  LAGRANGIAN  MULTIPLIERS  ARE  COMPUTED  IN  THIS 

C        SECTION. 

C 

C 

DEN1  =  A  ( 1 , 7  )  *ATERM  ( 1 )  -  A  (  4  ,  7  )  *  ATERM  (  2  >  +  A  <  7  r  7  )  *ATERM  ( 3 ) 
LAMBDA  ( 1 )  =  !  -T7*ATERM  <  1 )  +T8* ATERM  (  4  )  -T?:K ATERM  (55)  /DEN1 
LAMBDA <  4  >  =  < T7*ATERM ( 2 ) -T3:*:ATERM ( 6 ) +T?*ATERM  <  7 ) ) /DEN1 
LAMBDA<7)=<-T7*ATERM<3>+T3*ATERM<8>-T?*ATERM<?>  >/DE«l 
DEN2  =  A<2>10)*ATERM(10>-A<5»10!*ATERMUi:'+A<8.105 

*  *ATERMC12) 

LAMBH  A  ( 2  )  =  <  -T 1 0*ATERM  <  1 0  )  +T 1 1  *  ATERM  ( 1 3  )  -T 1 2*ATERM  ( 1 4  )  ) 

*  /DEN2 

LAMBDA  i  5  >  =  <  T 10* ATERM < 1 1 ) -Tl 1* ATERM (IS) +T12*ATERM C 16  >  > 

*  /DEN2 

LAMBDA(3)  =  (-T10*ATERM(12)+T11S:ATERM(17)-T12*ATERM<18)  > 
i  /DEN2 

DEN3=A  (  3 1 1 3 )  *  ATERM  '. 1 9  )  -A  <  a ,  1 3 )  *ATERM  ( 20  )  +  A  (  9  , 1 3  > 

*  *ATERM<21> 

LAMBDA<3)  =  <-T13*ATERM(1'?>  +  T14*ATERM(22J-T15*ATERM<23)  ) 

*  /DEN3 
LAMBDA<6)=(T13*ATERM<20)-T14*ATERM(24)+T15*ATERM!2S! ) 

i  /DEN3 

LAMBDA<9)»(-T13*ATERM(21)+T14*ATERM<26J-T1S*ATERMC2?) ! 

*  /DEN3 
C 

C 

C        THE  STATE  VARIABLES  ARE  DEFINED  HERE. 

C 

C 

Y(1>=PX 

Y<2)=PY 

Y ( 3  > =PZ 

Y(4)=PHI 

Y(5>=THETAB 

Y(6)=PSI 

Y(7)=DPX 
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y;3;=dpy 
y ( ? ) =dpz 

Y(10)=BPHI 
Y(11>=BTHETB 
Y(12)=BPSI 
DYU)=DPX 
BYC2/=DPY 
BY<3)=BPZ 
BY(4)=DPHI 
CY(5)=DTHETB 
BY(6>=DF'SI 
DY(7)=DDPX 
riY(S)=ODPY 
DY(9)=DDPZ 
BY<10>=BBPHI 
DY<il>=BBTHEB 
DY<12)=BBPSI 
C 

c 

C        SUBROUTINE  RILES  IS  THE  RUNGE-KUTTA-GILL  NUMERICAL 

C        INTEGRATION  ROUTINE  USED  TO  CALCULATE  THE  POSITION, 

C        VELOCITY,  ANB  ACCELERArION  Or  THE  BODY, 

C 

C 

CALL  R I LEG ( NED . I NCR , Y , BY , RQ ) 
C 
C 

C        THE  STATE  VARIABLES  ARE  PASSEB  BACK  TO  THERE  PHYSICS 
C        VARIABLES  IN  THIS  SECTION. 
C 
C 

"'X  =  Y(1) 

PY=Y(2) 

PZ=Yi3) 

PHI=Y<4) 

THETAB=Y<3> 

PSI=Y<6> 

DPX=Y<?> 

BPY-Y ( 3 ) 

BPZ=Y<9> 

BPHI=Y(10! 

BTHETB=Y(U) 

BPSI  =  YU2> 

BBPX=BY(?) 

BBPY=BY«S) 

DDPZ=BY<9> 

BBPHI=BYaO> 

BBTHEB=DY(11) 

BBPSI=BY(12; 
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c 

C        THE  DERIVATIVES  OF  THE  CONSTRAINT  EQUATIONS  ARE 

C        CALCULATED  AT  THE  NEW  BODY  POSITION,'  THE  COMMON 

C        TERMS  CONSISTING  OF  THESE  DERIVATIVES  ARE  ALSO 

C        COMPUTED  AT  THE  NEW  BODY  POSITION,   SUBROUTINE 

C        LEGVEL  CALCULATES  THE  VELOCITY  OF  THE  SUPPORT  LEG 

C        JOINT  ANGLES.   SUBROUTINE  LEQPOS  CALCULATES  THE 

C        JOINT  ANGLES  OF  THE  SUPPORT  LEGS  BASED  ON  THE 

C        POSITION  OF  THE  BODY  AND  THE  ENDS  OF  THE  SUPPORT 

C        LEGS , 

C 

C 

CALL  ACOEF 

CALL  ATERMS 

CALL  LEGVEL 

CALL  LEGPOS 

RETURN 

END 

C 

c 

C        SUBROUTINE  ACOEF  CALCULATES  THE  PARTIAL  DERIVATIVES 

C        OF  THE  CONSTRAINT  EQUATIONS. 

C 

C 

SUBROUTINE  ACOEF 

DIMENSION  A (? t IS ) » THETA< A » 6 > » TERM ( 1 7 ) 

REAL  L(4t6) 

INTEGER  P>Q»R 

C0MM0N/B2/THETA.L 

COMHON/B 1 2/PX r PY > PZr PHI jTHETAB.PS I 

CGMM0N/B13/P,Q?R 

C0MM0N/B16/A 
C 
C 

C        ALL  OF  THE  COEFFICIENTS  ARE  INITIALIZED  TO  ZERO. 
C 
C 


DO  100  KC=1t? 

DO  200  KD  =  1,10' 

A(KC;KD)=0,0 
200      CONTINUE 
100      CONTINUE 
C 
C 

C        SUBROUTINE  TERMS  COMPUTES  THE  COMMON  TERMS  USED 
C        IN  DEFINING  THE  A'S  (PARTIAL  DERIVATIVES  OF  THE 
C        CONSTRAINT  EQUATIONS), 
C 
C 
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CALL  fERMSCP.TERM) 


VALUES  ARE  ONLY  ASSIGNED  TO  THE  COEFFICIENTS  WITH 
NOH  ZERO  VALUES. 


A<1 

.1) 

=  1.0 

A<1 

t4) 

=  TERM 

1) 

AU 

,5) 

=  TERM 

•? ) 

A(l 

>6) 

=TERM 

3) 

AU 

,7) 

=TERM 

4 ) 

A(l 

-3) 

■TERM 

*3 ) 

AU 

,?) 

=  TERM 

6) 

A  (4 

»2) 

=  1.0 

A  (4 

,4) 

■TERM 

7) 

A(4 

5) 

=TERM< 

8) 

A  (  4 

»6) 

=TERM<9> 

AC4 

,7) 

=TERM(10) 

A(4 

»8> 

=TERMi 

11) 

A(4 

»?> 

=TERM< 

12) 

A(7 

3  -: 

=  1.0 

A  ( 7 

,5) 

«TERM< 

13) 

A<7 

•  6) 

■TERM! 

1 A  ) 

A(7 

.7) 

=TERM' 

13) 

A(7 

-8) 

«TERM< 

16) 

Aw 

9 ) 

=TERM> 

1 7 ) 

CALL 

.  TERMS(C 

. TERM ) 

A(2 

15 

=  1.0 

A '.2 

4) 

=TERM( 

1) 

A(2 

5) 

=TERM( 

2) 

A(2 

6) 

=TERM< 

3) 

A(2 

10 

)=TERf 

(4) 

A(2 

11 

)=TER!" 

(5) 

A<2 

12 

5=TERM 

( 6 ) 

A  (5 

2) 

=  1.0 

ACS 

4) 

»TERM< 

7) 

A  (3 

5) 

»TERM( 

8 ) 

A(5. 

6) 

=TERM< 

9) 

A<5i 

10 

>=TERM 

<  1 0 ) 

A(5> 

11)=TERM 

(11) 

A  ( 5 1 

12 

! -TERM (12) 

A<Si 

3) 

■  1.0 

ACS, 

5  ) 

»TERM< 

13) 

A<8> 

6) 

=TERM( 

14) 

A(Sj 

10 

) "TERM < IS) 

A<3> 

11 

>«TERMU6) 

A<8» 

12 

>=TERMU7) 

CALL 

TERMS (R 

j TERM) 

A<3.1)=1.0 
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A(3»4)»TERM(1) 

A(3>5)=TERM<2) 
A<3»6)=TERM<3) 
A<3>13>*TERM(4) 
A(3»14)«TERM(5) 

A(3.15)=TERM<6) 
A(6»2)»i.O 

A(6,4)=TERM<7> 

A<6,5)=TERM<3) 

A ( 6 , 6 ) -TERM ( 9 ) 

A<6t13)=TERM<10) 

A(6>14)=TERM<11) 

A<6flS)»TERM(12) 

A(9,3)=1.0 

A(9»5)=TERM<13) 

A(9>6)=TERM<14) 

A(9,13)=TERM(15) 

A(9>14)=TERM(16) 

A(9,15)=TERM<17) 

RETURN 

END 

c 
c 

C        SUBROUTINE  TERMS  COMPUTES  THE  NECCESSARY  TERMS  FOR 

C        CALCULATING  THE  PARTIAL  DERIVATIVES  OF  THE 

C       '  CONSTRAINT  EQUATIONS. 

C 

C 

SUBROUTINE  TERMS ( I i TERM) 

DIMENSION  TERM (17) : THETA < 4 • i  s 

REAL  L(4,6) 

C0MM0N/32/THETArL 

COMMON/ B12/PX , PY , PZ , PHI > THETAB i PS  I 
C 
C 

C        SOME  COMMON  TERMS  ARE  COMPUTED  HERE. 
C 
C 

TERMO 1  =  COS  < PS  I +  THET A  < 1 , I )  +  THET A ( 2 , 1 ) ) 

TERM02=SIN ( PS  I +THETA ( 1,13 +THETA ( 2  1 1 )  i 

TERM03=C0S(PSI+THETA<1»I) ) 

TERM04=SINCPSI+THETA(1.I> ) 

TERM05  =  C0S(THETA«3»I)+THETAi;4.  I  )  ) 

TERM06»SIN< THETA ( 3,1) +THETA (4,1)) 

TERM07  =  TERM05*L (4.1) +COS ( THETA (3,1!) *L C3» I ) +L  <2r I ) 

TERM08=-TERM06*L  (  -1  tl  )-SIN  <  THETA  <  3  , 1  )  )  *L  (  3  , 1 ) 

TERM09=C0S(PHI) 

TERM10=SIN(PHI) 

TERMli=COS<THETAB> 
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TERM12=SIN<THETAB) 

TERM13=C0S(PSI) 
TERM14=SIN(PSI) 


C        THE  TERMS  USED  IN  SUBROUTINE  ACOEF  ARE  CALCULATED 
C        HERE. 


TERM( 1  )=-TERM10*TERM01*TERM07-TERM10*TERM03.tL(  1,1) 

*  -TERM09*TERM11:*TERM02*TERM07-TERM09*TERM11 

*  *TERM04*L  (1,1 )  -TERM09»:TERM12*TERM06*L  (1,1) 

*  -TERM09*TERMi2«L<3.4) 
TERMC2)«TEKMi0*TERM12*TERM02*TERMO7+TERW10*TERM12 

i  *TERM04*L( 1 > I >-TERM10*TERHll:*TERMC'6*L(4, I ) 

*  -TERM10*TERM1 1*3 IN ( THETA ( 3  > I ) ) *L ( 3 , I ) 
XERM<3)--TERK09*TERM02*TERM07-TERM09*TERM04*L.<1»I> 

*  -TERM10*TERM11*TERM01#TERM07-TERM10*TERM11 

*  *TERM03*L(1,-I) 
TERM(4>»-TERM09*TERM02*TERM07-TERM10*TERM11*TERM01 

*  *T£RM07 
TERh(5)=TERM09:lcTERM(mTERM08-TERM10*TERMll*TERM02 

*  *TERM08-TERM10*TERM12*TERM05*L(4,I)-TERM10 

*  *TERM12*CQS(  THETA  <3»U  )*L(3,I) 
TERM(6)=-TERH09*TERM01*TERM06*L(4,I)+TERM10*TERM11 

*  *TERM02*TERM04*L (4,1) -TERM10*TERM12*TERM05 

*  *L ( 4 . I ) 

TERM i 7 ) =TERM09*TERM01*TERM07+TERM09*TERM03*L ( 1 ■ I ) 

*  -TERM10*TERM11*TERM02*TERM08-TERM10*TERM11 

*  -fcTERM04*L( 1 , I >-TERM10*TERM12*TERM06*L<4 ,1 ) 

*  -TERM10*TERM12*SIN(THETA(3,I));f:L(3,I) 
TERM(8)=~TERM09*TERM12*TERM02*TERM07-TERM09*TERM12 

*  *TERM04*L (1,1 ) +TERM09*TERM1 1 #TERK06*L (4,1) 

*  +TERH09STERM1 1*5  IN (THETA (3,1)) *L (3,1 ) 
TERM  (  9  )  =-TERM10*TERM02*TERM07-TERM10*TERM04'KL  (1,1 ) 

*  +TERM09*TERM11*TERM01*TERMC'7  +  TERM09*TERM11 

*  *TERM03*L(1,I) 
TERM(10)=-TERM10*TERM02*TERM07  +  TERM09*TERMll:*TERM01 

»  *TERM07 

TERM(11)=TERM10*TERM01*TERM08+TERM09*TERM11*TERM02 
*TERM08  +  TERM09*TERM12*TERM05:*L(4,I)+TERM09 

*  *TERM12*C0S(THETA(3,I)  )*L(3,I) 

TERM  (12)  =-TERM10*TERM01*TERM06*L  (4,1)  -TERMO?:KTERMl  1 

*  *TERM02!t:TERM06*L(4,I)+TERM09*TERM12:KTERM05 

*  *L ( 4 , I ) 

TERM'  13)=TERM11XTERM02*TERM07+TERM11*TERM04*L<1,I) 

*  +TERM12*TERH06*L (4,1 ) +TERM12*SIN ( THETA (3f I ) 

*  :«L  ( 3 , 1 ) 
TERH(14)=TERM12*TERM01*TERM07+TERM12*TERM03*L(1,T) 
TERM<1S)=TERH12*TERM01*TERM07 
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C***M 

C 

C 

c 
c 

c 
c 


TERMa6)*TERMi2*TERM02*TERM08-TERMll*TERM05*L<4»I} 

-TERMti*COS<  THETA  <  3 1 I ) ) *L ( 3  > I ) 
TERM  ( 17)»-TERMi2*TERK02*TERM06*L  ( 4 , I )  -TERM1UTERMQ5 

*L ( 4 1 1 ) 
RETURN 
END 


SUBROUTINE  ATERMS  IS  USED  TQ  CALCULATE  SOME  COMMON 
TERMS  INVOLVING  THE  CONSTRAINT  EQUATION  DERIVATIVES. 


SUBROUTINE  ATERMS 
DIMENSION  A<9.15).ATERM<27) 

C0MM0N/B16/A 
C0MM0N/B22/ATERM 


THE  COMMON  TERMS  ARE  CALCULATED  HERE, 


ATERMU)  = 
ATERM(2>= 
ATERM<3>= 
ATERM<4)» 
ATERM(5)» 
ATER«<6)3 
ATERM<7)= 
ATERM'.8)  = 
ATERM<9)= 
ATERM<10) 
ATERMU1) 
ATERMC12) 
ATERMU3) 
ATERMC14) 
ATERMdS) 
ATERM(lo) 
ATERM(17) 
ATERMU3) 
ATERMU9) 
ATERM<20) 
ATERMC21) 
ATERM(22) 
ATERM<23) 
ATERMC24) 
ATERMC25) 
ATERM<26) 
ATERM(27) 
RETURN 


4 . 8 )*A<  7  » 9 

1 »  8 ) *A  < 7 , ? 
I » 8 >*A<  4  >  9 
4 , 7 )*A  <  7 . 9 
4  . 7 ) *A<7 » 8 
1.7) *A (7.9 
1 ,7)*A(7?3 
1»7)*A<4>9 
1«7>*A<4.8 
C 5 . 1 1 ) «A ( S 
<2,11)*A<3 
(2»li)*A<S 
<S.10>*A<8 
<5,10)*A<8 
<2,10)*A<8 
<2,10)*A<8 
<2»10>*AC3 
<2,10)*A<5 
C6»14)*A<9 
(3»14)*A<9 
<3»14)*A<6 
C6»13)*A<9 
<6»13>*A<9 
<3,13)*A<9 
<3>13)*A<? 
( 3 » 13  > #A  C  6 
(3.13)*A<6 


)-A< 
)-A< 
>-A< 
>-A< 
)-A< 
)-A< 
)-A( 
>-A( 
)-A( 
,12) 
,12! 
,12) 
.125 
,11) 
.12) 
•  11) 
,12) 
.11) 
•15) 
,15) 
,13) 
,15) 
i  I'D 
, 15 ) 

,14) 

.15) 
.1-1) 


4 .9 ) *A< 

1,9>*A< 
1,9>*A< 
4 i 9 ) *A ( 
4,8)*A< 
1 , 9 ) *A ( 
1,8)*A< 
1 .9  >*A< 
1,S)*A< 
5,12 
2.13 
2,12 
5,12 
3,11 
-A (2, 12 
-A<2»11 
-A (2, 12 
-A<2,11 
-AC6.15 
-A (3. 15 
-AC3.1S 
-A(6,15 
-A<6.14 
-AC3.15 
-A  (3, 1-1 
-AC3, 15 
-A(3,14 


■A( 


/  7  8  .1 

7.8) 

4,8) 

7,7) 

7,7) 

7,7) 

7,7) 

4,7) 

4,7) 

>*AS  8 

)  *A  ( 8 

)*A(5 

)*A(8 

)  *A  ( 8 

)*A(3 

)*A<3 

)  *A  (  3 

>*A(5 

>*AC9 

)*A<9 

)  *A  ( 6 

>*A<9 

>*A<9 

)  *A  <  9 

)*A(9 

>*A<6 

)*A(o 


11) 
11! 
11) 
10) 
10) 
10) 
10) 
10) 
10) 
14) 
1-1) 
14) 
13) 
13) 
13) 
1  3  ) 
13) 
13) 


150 


END 
C*****  *  #  #  *  *  *  *  *  *  *  *  *  #  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  X  *  '*  *  *  *  *  *  %  *  *  *  *  *  :i; 

C 

C 

C        SUBROUTINE  RILEG  IS  THE  RUNGE-KUTTA-GILL  NUMERICAL 

C        INTEGRATION  SUBROUTINE  USED  TO  COMPUTE  THE  BODY 

C        POSITION  AND  FIRST  AND  SECOND  DERIVATIVES. 

C 

C 

SUBROUTINE  RILEG  (  NEC!  t H  t  Yi  DY  ,  Q  ) 

DIMENSION  A (23 

DIMENSION  Y(NEQ;  vOY'NEQ)  ,  Q<  NEC! ) 

A(1)=0.2?28?321SS134224 

A<2>=1.70?106781136547 

H2=0.5*H 
C 

c 

C        SUBROUTINE  LEGEQ  IS  WHERE  THE  STATE  EQUATIONS  ARE. 

C 

C 

CALL  LEGEQ''HEQ»Y»DY) 

DO  1.3  I=1»NEQ 

8=H2*BY(I)-G<I) 

Y<I)»Y(I)+B 
13       SKI)=Q(I)+3.0*B-H2*DY(I) 

DO  20  IJ~l,2 

CALL  LEGEQ <NEG»Y»DY) 

DO  20  1=1, NEQ 

B  =  A(IJ)*Crl*DY(I)-GKI>) 

Y(I)=Y(I)+B 
20       Q  ( 1 3  =Q  <  I )  +3  .  0*B-  A  <  I J  )  *H*D  Y  ( I ) 

CALL  LEGEQ < NEQ. YrDY) 

DO  26  1  =  1, NEC! 

8=0. 1666666666666666*:  ',H*DY<  I  j-2,0*Q<  I  )  ) 

Y< I)=Y(I)+B 
26      a(I)=a<I)+3*0*B-H2*BYfI) 

RETURN 

END 
C  *:  :K  *  ;i;  *  *  S; :k  **;(!****  *  Si  *  ;J-  :l:  *  X ;):  5(:  ;f  *  *  *  *  *  *  :K  *  *  *  *  *  *  :*  *  *  *  *  *  *  *  :K  *.  ;K : K  *  *  :k  *  *  *  :k .'S;  * 
C 

c 

C        SUBROUTINE  LESEQ  SUPPLIES  THE  STATE  EQUATIONS  TO 

C        THE  RILEG  SUBROUTINE. 

C 

c 

SUBROUT I NE  LEGEQ < NEQ i Y , BY ) 

D I MENS I ON  Y  <  NEQ ) » DY ( NEQ ) , A ( ? , 1 5 ) 

REAL  LAMBDA ( 9 ) > MB ; IXO » I YO • I ZO 

REAL  Mil.  M22  :■■  M33  >  M44  r  M43  ,hA6,  M5<1 .  M53  »  M64  ,  M66 

C0MM0N/B1/GR;MB 
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COMMON/B11,'IXO>IYO,IZO 

C0MH0N/B16/A 

C0MH0N/B17/LAHBDA 
C 
C 

C        SOME  COMMON  TERMS  ARE  CALCULATED  IN  THIS  SECTION. 
C 
C 

CPHI=C0S<Y<4> ) 

SPHI=SIN<Y<4>) 

CTHETB«C0S<Y(5) ) 

STH£TB"SIN<Y<5>) 

CPSI»C03<Y<6) ) 

SPSI=SIN<Y(6>> 

M11=MB 

M22=MB 

M33=MB 

M44»STHETB**2*<SPSI#*2*IX0+CPSI**2*IY0)+CTHETB**2*IZ 

M45=STHETB*CPSI*3PSI*UX0-IY0> 

M4o=CTHETB*IZ0 

M54=M45 

M55=CPSI«2:iaX0  +  SP3I**2*IY0 

M64=M46 

M66=IZ0 

01=0.0 

Q2=0.0 

Q3»0.0 

Q4-0.0 

«5=0.0 

06=0.0 

no  ioo  1=1,? 

G1=C11-LAMBIIA<I>*A<I,1> 
C!2=Q2-LAMBDA  ( I  )*A<  I  , 2  ) 
Q3=Q3-LAMBDA ( I ) *A ( I , 3 ) 
Q4=Q4-LAMBDA  ( "I  )  *A  ( I ,  A  ) 
Q5=Q5-LAMBD A ( I ) *A ( I *3 ) 
Q6=Qit-LAMBDA  ( I )  *A  <  1 ,  6  ) 
100  CONTINUE 

G3=Q3+MB*GR 

61=0,0 

G2=0.0 

G3=0.0 

G4=Y ( 1 0 ) *Y < 1 1 ) * ( 2 . Q*CTHETB*STHETB* ( SPS 1**2*1X0 

*  +CPSI**2*IYO-IZ0> ! 

*  + Y ( 1 0 > * Y ( 1 2 • * ( 2 . 0*STHETB**2*CP3 I*SPS1* ( IXO-I YO ) ) 

*  +Yai)**2*<CTHETB*CPSI*§PSI*(IX0-IY0)  ) 

*  +Y  (11)  *Y  ( 12  )  *  (  STHETB*v  CFSI«2-SPSI*:K2  5 

*  *<IXO-IYO)-STHETB*IZO> 

35«-Y<10)**2*<CTHETB*STHETB*<SPSI**2*IXO-ePSI**2*IY« 

i  -120)! 
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$ 


*  +r(10>*Y<12>*<STHETB*<CPSI**2-SPSI**2)*<IX0-IY0> 

*  +STHETB*IZO> 

*  - Y ( 11 > * Y ( 1 2 ) * ( 2 , 0*CPSI *SPS I  * < I XO- 1 YO ; ) 
36-- Y < 1 0 ) **2* ( STHETB**2*CPS I *SPS I  * ( I XO- 1  YO )  ) 

-Yaomai)*<STHETB*<CPSI**2-SPSI**2>*aX0-IYQ> 

*  +STHETB*IZO) 

*  - Y ( 1 1 ) **2* ( CPS I *SPSI * < I XO- I YO ) ) 
£iET=M44*M55*M66-M45M2*M66-M4.A«2»:M55 

THE  STATE  EQUATIONS  ARE  DEFINED  IN  THIS  SECTION. 


DY<1)»Y<7) 

0r(2)=Y«S) 

DY(3)=Y(?) 

DY(4>=Y(10> 

DY(5)=Y(11> 

riY<6)=Y(12> 

0Y(7>=<-Q1-G1>/M11 

DY(8)»<-Q2-G2)/«il 

DY(?)=(-Q3-G3)/M11 


C        THIS  SECTION  WAS  ADDED  BECAUSE  THE  MASS  MATRIX 

C        BECOMES  SINGULAR  WHEN  THE  BODY  ORIENTATION  ANGLES 

C       TEND  TOWARDS  ZERO. 

C 

C 

I F< DET ,LE  * ( 1 . OE-10 ) > THEN 

DY ( 1 0  I = ( -G4-Q4 ) / <  2 . 0* I ZO ) 

DYill)=(-G5-Q5)/IX0 

BY ! 1 2 ) = < -G6-Q6 ) / ( 2 . 0*120 > 
ELSE 

DY(10)  =  (M55*M6<4*<-Q4-Q4)-M4S*M66*<-Q5-QS! 

*  -M46*M55*<-G6-B6> )/BET 

DY  <  1 1  >  -  (  -M45*M66*  (  -Q4-G4  !  +  (  M4  4*M66-M4i«2  J  *  (  -H3-G3  ) 

*  +M45*M46*(-G6-G6> ) /DET 

DY  ( 12)  ■=  ( -M46*M55* ( -G4-G4 )  +M45*M46*  ( -05-85 ) 

*  •!•  (  M44*M55-M4S«2  )  %  <  -Q6-G6  )  )  /DET 
END  IF 

RETURN 
END 

C 

C        SUBROUTINE  LEGVEL  COMPUTES  THE  JOINT  YELOCITIES 

C        OF  THE  SUPPORT  LEGS. 
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SUBROUTINE  LEGVEL 

DIMENSION  A<9<  15)  >ATERM<27)  >0THETA<4,6)  ■•  ti Q .:  6,6) 

INTEGER  P,Q,R,AP,AR 

REAL  L<4>6) 

COMMON/BS/AP  > AR  r  DTHETA i DO. 

C0MMQN/B13/P»QrR 

C0MM0N/B16/A 

CQMM0N/B19/DPX»DPY>DPZ»DPHI,BTHETBfDPSI 

C0MM0N/B22/ATERM 
C 
C 

C        SOME  COMMON  TERMS  ARE  CALCULATED  HERE, 
C 
C 

Cl  =  - ( A ( 1 > 1 ) *DPX  +  A ( 1 > 2 ) KDPY+A (1.3) *DPZ+A U  >  4 ) *DPH I 

*  -r A  <  1  r  5  )  *DTHETB+ A  (1,6)  *BPS I ) 

C2  =  -  (  A  c 2  . 1 )  *BPX  f  A  (2,2)  *DPY  +  A  I  2  ,  3  )  *DPZ+A  (2,4;  *tlPH.I 
$      +A < 2 , 3 ) *DTHETB+ A ( 2,6) *DP3I ) 

C3  =  -  (  A  (  3  •  1 )  *DPX+A  (3,2)  *BPY+A  (  3  ,  3  )  *BPZ+A  (3,4  )  *DPHI 
t      +A  ( 3  i  5 )  *BTHETB+A  (3.6)  *DPSI ) 

C4"»-<A<45l>*DPX+AC4.2)*DPY+A(4»3)*DPZ+A(4»4)*DPHI 
J      +A  <  4  ,  5  )  *DTHETB+A  (  4»  6 )  *3PSI ) 

C5  =  -  (  A  (  5  »  1 )  *DPX+A  ("  5 ,  2  >  *BPY+A  ( 3  j  3  )  *OP'I+A  <  5  ■  4  )  *DPHI 

*  (-A  <  5t  5 )  *DTHETB+A  ( S  >  6 )  *BPSI ) 

C6=-  (  A  (  6  , 1 )  MPX+A  (6,2)  *BPY+A  (  6  »  3  )  *BPZ  +  A  (6,4)  KBPHI 
»      +  A  (  6  ,  5  )  *DTHET5  +  A  (6,6)  *BPSI ) 

C7  =  -  (  A  (  7  ,  1 )  *BPX+A  (7,2)  *BPY  +A  (  7  ,  3  )  KDPZ+A  (7,4)  *BPHI 

*  + A ( 7 • 5 ) *B THETB+ A (7,6) *DPSI ) 

CS=- ( A ( S , 1 ) *BPX+A (3,2 ) *0PY+A (8,3) *BPZ+A ( 3 , 4 ) XDPHI 

*  +A  (8,3)  KDTHETB+A  (  8,6)  *DPSI ) 

C?  =  - ( A ( 9 , 1 ) *BPX  +  A ( ? , 2 ) *BPY+A (9,3)  *BPZ+A (9,4) *BPH I 

*  tA(?,5;*0THETB  +  A(9-6):|:DPST ) 
C 

C 

C        THE  ANGULAR  '-,'ELGCIT  [ES  OF  THE  SUPPORT  LEG  JOINTS 

C        ARE  CALCULATED  IN  THIS  SECTION. 

C 

C 

DEN1 =A  (1,7)  *ATERM  ( 1  )  -A  (  4  ,  7  )  *ATERM  (  2  )  +A  (  7  ?  7  )  :SATERM  ( : 
DTHETA ( 2  •  P  )  =  ( C1*ATERM ( 1 ) -C4*ATERM  < 2 ) fC7*ATERM ( 3 ) ) 
T>  /DEN1 

DTHETA  (  3  ,  P  )  =  (  -CI  *ATERH  (  4  )  t-C4*ATERM  (  6  )  -C7';:ATERM  (  3  )  ) 
»  /DEN1 

BTHETA<4»P)=<C1*ATERM<5>-C4*ATERM<7>+C7*ATERM<?) ) 

*  /DEN1 

0EM2=A>  2,10)*ATERM(10)'-A(3, 10>#ATERM< 11)+A<8»10) 

*  *ATERM(12) 

DTHETA(2,Qi  =  (C2*ATERM(10)-C5*ATERM(U)+C8*ATE='MC2- 

*  /DEN2 

DTHETA  (  3  ,  Q  )  =  (  -C2S:ATERM  ( 13  )  +C5*ATERM  (15)  -CSfcATERM  ( 1 ," 
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*  /DEM2 

DTHETA  (  4  ,d  >=»  (  C2*ATERM  ( 14  )  -C5*ATERM  <  1 6  !  +C3*ATERM  (  1 3  >  > 
»  /BEN 2 

DEN3  =  A(3>13}*ATERNa?>-A<6.13>*ATERMC20>+A<?jl3> 

*  *ATERM«21> 

DTHETA  <  2  ,  R  )  =  ( C3*ATERM  ( 1?  >  -C6*ATERM  (  20  )  +C?*ATERM  (  21 >  ) 

*  /DEN3 

DTHETA(3JR)  =  (-C3*ATERM';22)+C6»:ATERM(24)-C9ii:ATERMf26) 

*  /DEN3 

DTHETA  (  4  ,R)»(  C3*ATERM  (  23  )  -C6*ATERM  <  25  1  +  C?:t:ATERM  (27)  ) 

*  /DEN3 
RETURN 

END 

c 
c 

C        THE  SUPPORT  LEG  JOINT  ANGLES  ARE  COMPUTED  IN 

C       THE  SUBROUTINE  LEQPOS. 

C 

C 

SUBROUTINE  LEQPOS 

DIMENSION  THETAC4  >6)  >XW<6> >YW(6)»2UC6) 

INTEGER  P,Q»R 

REAL  L<4>6) 

C0MM0N/B2/THETApL 

CQMMON/B 12/PX i PY  7  PZ t PHI ■ THETA8»PSI 

C0MMON/B13/P.Q.R 

C0MM0N/B23/XW  ?  YUI  r  ZW 

C0MM0N/B6/N.K 
C 

c 

C        SOME  COMMON  TERMS  ARE  COMPUTED  HERE, 
C 

c 

cphi=cos(phi> 

sphi=sin(rhi) 

cthetb=cos(thetab:i 

sthetb=sin<;thetab> 

cpsi=cos(psi> 

spsi=s1n(psi) 


'■'-  THE  END  POSITIONS  OF  THE  SUPPORT  LEGS  IN  THE 

C  BODY  CXO.YOfZO)  COORDINATE  SYSTEM  ARE  CALCULATFD 

C  IN  THIS  SECTION, 

C 

c 

DO  100  J=P,R,2 

X=  (  CPHI*CPS  I  -SPH I  *CTHETB*SPS  I )  *  <  XU  (  J  )  -PX  )  + 
4     (SPHI*CPSI+CPHI*CTHETB*SPSI)*<YU(J)-PY)+ 
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*  < STHETB*SPSI ! * ( ZW C J ) -PZ) 

Y=<  -CPHI*SPSI-SF'HI*CTHETB*CPSI )  .*  (  XW<  J )  --RX  )  + 
$  (  -3PHI:KSPSI  tCPHI*i:THET3*CPSI  >*(  YW  ( J )  -FY  >  + 
$     ( STHETB*CPSI >*< ZU< J ! -PZ) 

2= ( SPHIKSTHETB 5  * ( XU ( J 1 -PX )  + 

*  (  -CPHI *STHETB  )  *  t  YU  (  J  )  -PY  )  + 

*  CTHETB*<ZW(J)-PZ) 

C 

C        the  SUPPORT  LEGS  JOINT  ANGLES  ARE  CAI  CULATED  HEPE 

C 

c 

Cl=COS<THETACl.J)  i 
S1  =  SIN<THETAU>  J)  ) 

THETA(2»J)»ATAN(<'C1*Y-81*X>/<C1*X+S1*Y'-L<i»j)  )) 
C12=C0SaHETA«l-J)+THETA(2;J>) 
S12=SIN  < THETA i 1, J J+THETA ( 2 > J ) ) 
TERM© 1= ( X-L ( 1 1 J ) *Ci-L<  2  >  J  >*C12 ) /C 1 2 
C4= < TERM01**2+Z**2-L ( 4 » J ) **2-U < 3 • J ) **2 ) 
S  /(2.0*LC3»J)*L<4»J)) 

S4=S0RTC  1.0-0-1**2) 
THETA 1 4 , J ) =ATAN2 ( 34 , CI } 
C2=C0S(THETA(2.J) ) 
TERM02=L  (At  J)  *S4*  (  C 12*X+S1 2*Y-L  (  1 1  J  )  *C2-I-  ( 2 1 J  >  ) 

*  +Z*(LC4»J)*C4+L(3>J">) 

TERM03— <L<4.J)*C4+L<3»  J)  )*<Ci2*X+S12*Y-L(l» J>*C2 

*  -L(2>J)>+Z$34*L(4f J) 
THETA ( 3 1 J ) = AT AN ( TERM02/TERMO3 > 

100      CONTINUE 
RETURN 
END 

c 
c 

C        SUBROUTINE  ARM  CONTROLS  THL  DYNAMIC  COMPUTATION 

C        FOR  THE  NON  SUPPORT  LEGS. 

C 

C 

SUBROUTINE  ARM 

DIMENSION  THETA (  1.6) rBTHETA<4>6) »DDTHET< »»6)  »BG< 

DIMENSION  Y<6) ,DY<6) 

INTEGER  AP.AR 

REAL  INCR.L(4j6) 

COMMON/B2/THETA.L 

C  0  M  M 0 N /  B 8  /  A  P  >  AR  >  B THET  A  r  D  Q 

COMMCN/BIO/INCR 
C 
■C 

C        NEQ  IS  THE  NUMBER  OF  FIRST  ORDER  EGUATIONS  US^D 
C        IN  THE  RUNGE-KUTTA-GILL  NUMERICAL  INTEGRATION ""' 
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C        SUBROUTINE  RIARM. 
C 

c 

NEQ=6 
C 
C 

C        THE  STATE  VARIABLES  FOR  THE  NON  SUPPORT  LEGS  ARE 
C        DEFINED  IN  THIS  SECTION. 
C 
C 

DO  100  J=AP.AR,2 
YU)=THETA(2.J) 
Y<2!=THETA<3.J) 
Y<3>»THETAC4»J) 
Y(4)=DTHETA(2jJ) 
Y(5>»BTHETA(3> J) 
Y(6)=DTHETA(4.Jj 
IiY<l)=DTHETA<2»J) 
DY<2)=DTHETA<3>J> 
DY(3)=DTHETA<4,J> 
DY<4)«tDDTHETf2f  J) 
DY(5>=DDTHET(3»J> 
DY(6)=DDTHET(4j.J) 
C 
C 

C        SUBROUTINE  RIARH  IS  THE  NUMERICAL  INTEGRATION 
C        SUBROUTINE  USED  TO  COMPUTE  POSITION,  VELOCITY. 
C        AND  ACCELERATION  OF  THE  NON  SUPPORT  LEG  .JOINT'";. 
C 
C 

CALL.  RI  ARM  <  J  ,  NEC? ,  INCR  »  Y  •  DY  .  DQ  ) 
THETA<2>J>»YU) 
THETA<3.-J)=Y(2) 
THETA(4.J>=Y<3> 
DTHETA(2,J)=Y(4) 
CTHETA(3f J)=Y£5) 
DTHETA<4jJ)=Y(6> 
riDTHET(2>J)=DY(4) 
DDTHET(3iJ>»DY(5) 
DDTHET<4;J)=DY(6) 
100      CONTINUE 
RETURN 
END 

c 

C        SUBROUTINE  RIARM  IS  A  RUNGE-KUTTA-GILL  NUMERICAL 
C        INTEGRATION  ROUTINE  USED  FOR  CALCULATING  THE 
C        DYNAMICS  FOR  THE  NON  SUPPORT  LFGS. 


157 


SUB  R  OUT  I N  E  R I A  RM  <  J  >  NEC!  t  H  t  Y  •  D  Y  i  Q  3 

DIMENSION  A(2i 

DIMENSION  YCNEQ) rDYCNEQ) ?Gii»6> 

A<i>=».2?28932188:t  34524 

A<2>=1, 707 1067811 36547 

H2=0.5*H 
C 
C 

C        SUBROUTINE  ARMEQ  SUPPLIES  THE  EQUATIONS  OF  STATE 
C        FOR  THE  NON  SUPPORT  LEGS- 
C 
C 

CALL  AKMEGKJfNEQ.Y.BY) 

DO  13  I=1;NEQ 

B=H2*DY<I>-CHJjI) 

Y(I)=Y(I)+6 
13      Q( Jf I >=Q ( J? I >  +3 . 0*B-H2*BY< I ) 

DO  20  IJ=--1»2 

CALL  ARMEQ  ( J  t  K'EQ  .  Y  •  BY  ) 

DO  20  1=1. NEQ 

B«A ( IJ ) * ( H*DY ( I > -0  C J  > I ) ) 

Y(I)=Y(I)+B 
20       Q  (  J  t  I  >  =Q  ■■  J  >  I )  +3 .  0*B-  A  ( IJ  )  *H*B  Y  <  I ) 

CALL  ARMEQ  < J  > NEQ » Y » BY ) 

DO  26  1=1  >  NEQ 

B=0 . 1666666666666666* ( H*BY ( I ) -2 , 0*Q <  J > I ) > 

Yt I>=Y(I>+B 
26      Q ( J i I ) =Q ( J . I ) +3 , 0*B-H2*BY t I ) 

RETURN 

END 

C 

C 

C        3UBR0UITNE  ARMEQ  SUPPLIES  THE  STATE  EQUATIONS  USED 

C        BY  SUBROUTINE  RIARM. 

C 

c 

SUBROUT I NE  ARMEQ ( J  t NEQ  7 Y  ?  DY  i 

DIMENSION  T(4r6) >THETA<4»6) »YCNEQ) »BY<NE<3) >BBTHET<4»6> 

REAL  M<4>6)fLM<4»4)»IX<4f6) »IY<4>6)iI2<4>6)»L(4.6)fMB 

REAL  M11.»M22.M23»M33 
C0MM0N/B3/M.LM. IX. IYiIZ 
C0MM0N/B2/THETA*L 
C0MM0N/B1/GR.MB 
CCMM0N/B21/T 


SOME  COMMON  TERMS  ARE  CALCULATED  HERE. 
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C3=C0S(Y«2) > 

S3=SIN<Y£2>) 

C4=C0S(Y(3>> 

S4=3IN'Y<3  ■  ) 

C34«COSCY<2>+Y<33 ) 

S34«SIN<YC2)+Y<3>) 

Ml 1«M  <  Z, J)*I_M<  2»  J) **2+M<  3 1 J ) *( C3*LM ( 3 « J ) +i.<  2  r J ) ) *«2 

*  +M ( 4  j  J > * < C34*LM ( 4 . J ) +  C3*L ( 3  t J  >  +L t 2  r J  i ) **2  +  IZ ( 2 , J ) 

*  +  IX  ( 3 1 J )  *S3**2+1  Y < 3 >  J)  *C3#*2+i: X  (  4 .  J  >  *S34**2 

*  +IY(4>J>*C34**2 

M22  =  M  ( 3  >  J  )  *LM  (  3  i  J  1  **2+M  <  4  •  J  )  *  (  LM  (  4  .  J  )  «2 
9      +2.0*C4*t.<3.J>*l.M<  4.  J;+L(3.J)*:('2)  +  IZ(3jJ)  +  I2(4.  J) 
M23=M(4»  J>*(LM(4f  J)*»:2+C4*L(3t  J)*I.M<  4;  J)  )  +  I2(4fJ) 
M33»M ( 4 , J ) *LM (4 » J) **2+ 1 Z ( 4  > J ) 
S1.=?Y <  4 ) *Y <  5 )  *  ( -2 .  0*M < 3 1  J)  *S3*LM  ( 3 » J > * i  C3*LM  ( 3 » J ) 

*  tL  (  2  »  J )  )  -2  .  0*M  ( 4  i  J  >  *  (  C34*S34*LM  <  4  ,  J  )  *:*2  +  03*S3 

*  *L ( 3  » J ) «2  +  SIN ( 2 . 0*Y ( 2 ) +Y  f 3 ) ) *L ( 3  i J ) *LM ( 4 , J >  +  S34 
*L  (  2  ,  J )  *LM  (  4  7  J  )  +2 ,  0:SS3*L  (  2  t  J  )  *L  <  3  7  J  )  ) 

*  +  2.0*(IXi3.  J;-IY<3.J>}*C3*S3  +  2.0*CIX(4,.i>-IY(4.J>  ) 

*  *C34*S34>+ 

*  Y  (  4 )  *Y  ( 6 ) ;):  (  -2 ,  0*M  ( 4 ,  J >  #S34*LM  ( 4  >  J )  *  ( C34*LM  (  4  .  J ) 

*  +C3*L<3»  J) +L( 2* J ) ) +2 . 0* < I X  < 4  > J ) - 1 Y ( 4  7  J > 5 *C34*S34 ) 
B2=Y  ( 5 )  *  Y  ( 6  >  *  ( -2 ,  0*M  <  4 7  J )  *34*L  ( 3>  J >*I_M < 4 » J >  >-Y <S )  **2 

*  * ( M ( 4 1  J ) *S4*L ( 3  7  J ) *LM  <  4 1  J ) ) 

G3=Y  <3)*Y<  6 >*<-2 . OCT <4>  J  > *S4*L ( 3 . J ) *LM ( 4 7 J ) ) 

(31=0.0 

Q2=Y(4)**2*(M<3»J)*S3*LM<3»J)*CC3*LH<3f J)+L<2»J) ) 

*  +M  i  4  j J ) * ( C34*S34*LM C  4  > J ) **2+S I N ( 2 , 0*  Y  < 2 ) 

«      +Y ( 3 ) >  *L ( 3  *  J) *LM ( 4 1 J ) +S34*L ( 2 » J ) *LM <  4 1 J )  + 

*  C3*S3:*L ( 3 7 J ) **2+83*L <2.J)*L<37j)J-(IX(3.Ji 

*  -IY(3-J)  >*C3*S3-<IX(4.J>-IY<4»J)>*C34*S34>- 
i  M ( 3  7  J ) *GR*C3*LM ( 3 » J ) -M ( 4 , J ) *BR* <  C34*LM ( 4t J) 

*  +C3*L<3»J>) 

133= Y  <  4  )  **2*S34 %  ( M  <  4 » J) *LM < 4>  J ) *< C34*LM < 4»  J )  4-C3*L (3 r  J) 
$      +1.(27  J)  >-<IX<4»J>-IY(4! J))*C34)  + 
S      Y  (  5  >  **3*  ( -M ( 4  7  J )  *S4*L  (3>  J  )  *LM «  4  7  J  > )  - 

*  M ( 4  7  J ) *SR*C34*LM ( 4  7  J ) 

C 

C        THE  STATE  EQUATIONS  ARE  DEFINED  HERE, 
C 

c 

DY(i>=Y(4) 
DY(2)=Y<S) 
DY-3>=Y<e> 

DY(4)=(T(27J)-Q1-G1)/M11 

UY(5)'=i;M33*<T<3rJ)-02-e2)-M23*(T<4,J>-Q3-G3)  ) 
t  /<H33*M22-M23**2) 

DY(6>»(M22XfT(4»J)-C!3-63  5-M23*<T<3»J)-Q2-Q2)  ) 
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*        / < M33*M22-M23**2 ) 

RETURN 
END 

C 

c 

C        SUBROUTINE  XY2  CALCULATES  THE  END  POSITIONS,  IN  THE 

C        WORLD  (XUjYWtZU)  COORDINATE  SYSTEM?   OF  THE  LEGS 

C        THAT  ARE  MAKING  THE  TRANSITION  F'.'OM  NON  SUPPORT  TQ 

C        THE  SUPPORT  PHASE, 

C 

C 

SUBROUTINE  XY2 

INTEGER  AP>AR 

REAL  L<4f6> 

DIMENSION  TH£Tft<4>6)  >BTHETA<4;6)  jB8(6»6)  fXW<6)  ■YWiiS! 

DIMENSION  ZW<6) 

C0MH0N/B2/THETA?L 

COMMON/BS/AP  > AR , DTHETA , DQ 

C0MM0N/B12/PX , PY , PZ  >PHI » THETAB  > PSI 

C0MH0N/B23/XW  t YW , ZU 
C 
C 

C        SOME  COMMON  TERMS  ARE  CALCULATED  HERE. 
C 

c 

CPH-|>COS<PHI) 

3PHI=SIN(PHI) 

CTHETB=COS< THETAB) 

STHETB=3IN(THETAB) 

CPSI=COS(PSI) 

SPSI=SIN(PSI> 

TERii01  =  CPHI*CPSI-SPHI:*CTHET3*SPSI 

TERM02=-CPHI*SPSI-SPHI*CTHETB*CF'SI 

TERM03=3PHI:*STHETB 

TERM04=SPHI*CPSI+CPHI*CTHETB*SPSI 

TERM05=-SPHI*SPSI+CPHI*CTHETB*CPSI 

TERM06— CPHI*STHETB 

TERM07=STHETB#SPSI 

TERM08=STHETB*CPSI 

DO  100  J=.AP,ARr2 

C1«C0S<THETA(1»J) ) 

S1=SIN<THETAC1»J) ) 

C3=C0S(THETA(3,J>  > 

S3=SIN<THETA(3iJ) ) 

C12  =  C0S ( THET A  < 1 , J ) +THETA  C  2  ? J  > > 

S12»SIN<THETAU»J)+THETrt<2»J)) 

C34=C0S ( THETA (3» J) +THETA • 4 • J ) > 

S34=SIN ( THETA  < 3 • J ) +THETA ( 4  ,  J  )  ) 
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c 

C        THIS  SECTION  CALCULATES  THE  LEG  END  POSITIONS  IN 

C       THE  BODY  (XOtYOrZO)  COORDINATE  SYSTEM. 

C 

C 

X*Ci2*<L<4»J)*C34+L(3»J)*C3+L<2»J) >+L<l i J)*C1 

Y=S12* ( L ( 4 , J  i *C34+L ( 3 . J ) *C3+L <2>J>)+L(l7j>*Sl 

Z=-L ( 4> J >  *S34-L ( 3 , J ) *S3 
C 
C 

C        THE  LEG  END  POSITIONS  IN  THE  UORLB  (XW.YU.ZW) 
C        COORDINATE  SYSTEM  ARE  COMPUTED  HERE, 
C 
C 

XW  <  J  )  =TERh01*X  +  TERM02»:Y  +  TERM03*Z+PX 

YU<J>»TERH<54*X+TERM05*Y+TERM06*Z+PY 

ZWi:j>=TERf10?*X  +  TERrtOS*Y  +  CTHETB*Z+PZ 
100      CONTINUE 

RETURN 

END 

ex********************  ******  He******************************** 
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INPUT  FILE  'GAIN, DAT' 


■'Ki"K2>  =  '  (0,304 
'KM (3)=' ,0,167 
'KM (4)=' .0.0448 
'RJ(2)='.0 .511 
' RJ ( 3 ) *'  1 0 . 417 
'RJ<4>=' .0,112 
'KD='  ,30,0 
'KP=' , 100.0 
'KI=' ,160,0 
'PR0P0R-'  .0.7 


INPUT  FILE  'BODY. DAT' 


' KM= ' ?  0 . 62 

'KD=' ,17,3 
'KP=' -30,0 
'KI=' ,25,0 
'BEL*'  »i.OE- 


The  input  data  file  'INPUT.DAT'  contains  the  information  in 
Table  3.1. 
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ABSTRACT 


This  paper  investigates  the  use  of  polynomial  spline 
functions  to  guide  a  walking  robot.   The  use  of  spline 
functions  allows  for  small  amounts  of  input  data  to  be 
converted  into  smooth,  continuous,  paths  to  guide  the 
walking  robot. 

The  kinematic  and  dynamic  equations  are  developed  for 
a  six-legged  walking  robot.  The  mathematical  model  is  of  a 
robot  with  a  flat  cylindrical  body  with  six  three  degree  of 
freedom  legs  spaced  uniformly  around  the  body. 

A  recursive  formula  is  developed  for  computing  the 
spline  curve,  thus  eleminating  the  need  for  performing 
matrix  inversions.   Also  developed  is  a  method  for 
constructing  a  spline  curve  thru  a  large  number  of  data 
points  using  only  a  few  data  points  at  a  time. 

The  use  of  polynomial  spline  functions  as  inputs  to 
the  control  system  allows  the  use  of  feedforward  control. 
By  using  a  feedforward  loop,  the  control  system  has  the 
ability  to  anticipate  the  input  signal.   A  method  is 
developed  that  allows  the  supporting  legs  to  act  in  unison 
to  drive  the  body  along  its  correct  path  by  using  the  body 
position  and  orientation  as  inputs  to  the  control  system. 

The  equations  which  describe  the  dynamics  of  the 
walking  robot,  along  with  the  recursive  spline  mathematics, 


was  implemented  in  a  computer  simulation.   Performace  of  the 
simulation  are  presented,  as  are  recomendations  for  further 
study. 


■ 


